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I. INTRODUCTION 



The study of robotics is a fairly new area in the fields of science 
and engineering which has fundamentals that can be traced back to 
many basic disciplines, such as electrical engineering, mechanical 
engineering, and computer science. The mechanical engineering 
interest in the study of robotic manipulators can be traced back to as 
early as the 1940s, although the first industrial robots were not intro- 
duced until the late 1950s and early 1960s [Ref. 1]. Mechanical engi- 
neers were mostly interested in two areas of robotics: the control of 
the linkage and the study of its motion. While these two areas are 
interrelated, this thesis focuses on the second: the study of the motion 
of the linkage. 

The study of robot motion (dynamics) is further divided into the 
study of arm dynamics and robot arm kinematics. Both these divisions 
are subdivided into a direct and an inverse problem. The direct 
dynamics problem is the calculation of link state variables such as 
acceleration, velocity, and joint angles from the known joint torques. 
The inverse dynamic problem assumes state variables are known and 
the joint torques are to be calculated. In the kinematics, the direct 
problem is the determination of the end effector position and its 
orientation from a given set of link geometries and joint angles. The 
inverse problem is the calculation of the link geometries and joint 
angles for a given end effector position. 
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Various methods to solve the inverse kinematic problem use 
matrix transformations, inversion, and multiplication, and as such are 
subjected to singularity problems [Ref. 2]. Singularity occurs when two 
successive links are aligned (i.e., the angle between the links is 0 or 
180 degrees). When a singularity occurs, the matrices involved are also 
singular and not invertible. 

A method to avoid the singularity problem was developed at the 
Naval Postgraduate School using Newtonian dynamics in terms of a 
globally fixed coordinate system [Ref. 3]. This method treats each link 
as a free body with the forces and moments applied at the joints and 
the use of Newton’s law to derive the equations of motion. In this 
thesis, the development of the free body analysis approach to rigid 
manipulators dynamics is continued and extended to three dimen- 
sions and including gravitational effects. 
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II. PROBLEM STATEMENT 



Theoretical dynamic approaches are well developed and have 
been extensively used with simulations. However, in manipulator 
studies, these accepted methods all have a singularity that arises when 
two successive links are aligned. This does not allow the motion to be 
simulated [Ref. 3]. This shortfall cannot be tolerated in the modelling 
of an actual robot or robotic manipulator because it is of the utmost 
importance to accurately know the arm position at all times (for real- 
time control purposes) [Ref. 2]. Therefore, the problem directed by 
this thesis research project was: 

Continue the development of the Newton Euler free-body approach 
for a three-link manipulator extended to three-dimensional motion 
and including gravitational effects. In addition, compare simulation 
results with those of an actual robotic arm. [Ref. 4] 
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III. BACKGROUND 



A. THEORY 

The basic theory for the Newton Euler free-body analysis to over- 
come the singularity problems of conventional robot dynamics was 
developed by Sadrettin Altinok [Ref. 3]. The theory of treating each 
link of the manipulator as a free body with respect to a global coordi- 
nate system leads to the following 27 equations for a three-link 
manipulator. 



FXO + M1*AX1 - FX1 = 0 



( 1 ) 



FYO + M1*AY1 - FY1 = 0 



( 2 ) 



FZO + M1*AZ1 - FZ1 = -WG1 



(3) 



AX1 + RB1Z*\VD1Y - RBIXnVDIZ = AXO - MICO 



(4) 



where MICO = (X component of) 



WD1 X RBG1 + W1 X fWl X RBG1) 



(4a) 



AY1 - RB1Z*WD1X + RB1X*WD1Z = AYO - MJCO 



( 5 ) 



where MJCO = (Y component of equation 4a). 
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AZ1 + RBY1*WD1X - RB1Z*WD 1Y = AZO - MKCO (6) 

where MKCO = (Z component of equation 4a). 

RB1Z*FY0 - RB1Y*FZ0 - IXXT1*WD1X + IXYT1*WD1Y + 
IXZT1*WD1Z - RA1Z*FY1 + RA1Y*FZ1 = T1X - TOX (7) 

-RB 1 Z*FXO + RB1X*FZ0 + IXYT1*WD1X - IYYT1*WD1Y + 

IYZT1*WD1Z + RA1Z*FX1 - RA1X*FZ1 = T1Y - TOY (8) 

RB 1 Y*FXO - RB1Z*FY0 + IXZT1*WD1X + IYZT1*WD1Y - 
IZZT1*WD1Z - RA1Y*FX1 + RA1X*FY1 = T1Z - TOZ (9) 

FX1 + M2*AX2 - FX2 = 0 (10) 

FY1 + M2*AY2 - FY2 = 0 (11) 

FZ1 + M2*AZ2 - FZ2 = -WG2 (12) 

-AX1 - RA1Z*WD1Y + RA1Y*WD1Z + AX2 + RB2Z*WD2Y - 

RB2Y*WD2Z = MIC 1 - MIC2 (13) 

where MIC1 = (X component of) 

WD1 X RA1 +W1X1W1X RA1) (13a) 
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where MIC2 + (X component of) 



WD2 X RB2 + W2 X fW2 X RB2) (13b) 

-AY1 + RA1Z*WD1X - RA1X*WD1Z + AY2 - RB2Z*WD2X + 

RB2X*WD2Z = MJC 1 - MJC2 ( 1 4) 

where MJC1 = (Y component of equation 13a) and where MJC2 = (Y 
component of equation 13b). 

-AZ1 - RA1Y*WD1X + RA1X*\VD1Y + AZ2 + RB2Y*WD2X - 

RB2X*WD2Y = MKC 1 - MKC2 (15) 

where MKC1 = (Z component of equation 13a) and where MKC2 = (Z 
component of equation 13b). 

RB2Z*FY1 - RB2Y*FZ1 - IXXT2*WD2X + IXYT2*WD2Y + 
IXZT2*WD2Z - RA2Z*FY2 + RA2Y*FZ2 = T2X - T1X ( 1 6) 

-RB2Z*FX1 + RB2X*FZ1 + IXYT2*\VD2X - IYYT2*WD2Y + 

IYZT2*WD2Z + RA2Z*FX2 - RA2X*FZ2 = T2Y - T1Y (17) 

RB2Y*FX1 + RB2X*FY1 + IXZT2*WD2X + IYZT2*WD2Y - 

IZZT2*WD2Z - RA2Y*FX2 + RA2X*FY2 = T2Z - T1Z (18) 
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FX2 + M3*AX3 = 0 



(19) 



FY2 + M3*AY3 = 0 (20) 

FZ2 + M3*AZ3 = -WG3 (21) 

-AX2 - RA2Z*WD2Y + RA2Y*WD2Z + AX3 + RB3Z*WD3Y - 

RB3Y*WD3Z = MIC3 - MIC4 (22) 

where MIC3 = (X component of) 

WD2 X RA2 + W2 X fW2 X RA2) (22a) 

where MIC4 = (X component of) 

WD3 X RB3 + W3 X fW3 X RB3) (22b) 

-AY2 + RA2Z*WD2X - RA2X*WD2Z + AY3 - RB3Z*WD3X + 

RB3X*WD3Z = MJC3 - MJC4 (23) 

where MJC3 = (Y component of equation 22a) and where MJC4 = (Y 
component of equation 22b). 

-AZ2 - RA2Y*WD2X + RA2X*WD2Z + AZ3 + RB3Y*WD3X - 

RB3X*WD3Y = MKC3 - MKC4 (24) 
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where MKC3 = (Z component of equation 22a) and where MKC4 = (Z 
component of equation 22b). 

RB3Z*FY2 - RB3Y*FZ2 - IXXT3*WD3X + IXYT3*WD3Y + 

IXZT3*WD3Z = - T2X (25) 

-RB3Z*FX2 + RB3X*FZ2 + IXYT3*WD3X - IYYT3*WD3Y + 

IYZT3*WD3Z = - T2Y (26) 

RB3Y*FX2 - RB3X*FY2 + IXZT3*WD3X + IYZT3*WD3Y - 

IZZT3*WD3Z = - T2Z (27) 



B. COMPUTATION 

The Dynamic Simulation Language (DSL) was used to simulate the 
direct dynamics problem. A matrix (MATA, a 27 x 27 matrix) was cre- 
ated from the known coefficients of the unknown variables in the pre- 
vious 27 equations. This was possible due to the assumption that the 
change in the inertia of the links, the link velocities, and the joint 
position must be extremely small during a simulated step interval and 
can thus be treated as being constant during the step. In the direct 
dynamics problem, a vector (MatB, 27 x 1) was generated from the 
following components: joint torques, the centers of gravity of the link, 
and the calculated cross-products of angular velocities (Figure 1). This 
gave a system of equations in the form of equation 28: 
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A * x - b 



(28) 



where A and b are known and x is an unknown vector (which contains 
the forces at the link ends and the angular and linear accelerations of 
each link). The IMSL routine LEQT2F (a linear equation solver) was 
used to obtained the vector x. [Ref. 3 and Ref. 5] 

In the inverse problem, MATA was developed the same as in the 
direct problem as discussed above, but now vector x was known (from 
the history of position data) and vector b was found by straight matrix 
multiplication of equation 28. [Ref. 3 and Ref. 5) 

The previous work used transformation matrix operations to 
obtain joint orientations [Ref. 3). This was done because joint orienta- 
tion cannot be obtained directly from the integration of rotational 
velocity [Ref. 6 and Ref. 7]. The approach used in this thesis was one of 
simple geometric relationships. It was reasoned that the linear accel- 
erations were either known or calculated from the matrix operation of 
LEQT2F (equation 28). These linear accelerations were the linear 
accelerations of the link center of gravities with respect to the earth 
fixed (inertial) coordinate system which were integrated to obtain the 
linear velocities. The velocities were further integrated to obtain the 
position of the center of gravities. 

Knowing the earth fixed coordinates of the centers of gravity, and 
that each link can be represented by a rigid body [Ref. 8], it was 
known that the center of gravity must be on the vector from the 
inboard joint to the end of the link, or next joint (Figure 2). Once the 
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Figure 2. Link Geometries 
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vector was known to the center of gravity, the directional cosines 
were calculated from the following three equations for each link. 



DRCOSX = [LCOGX - JX(inboard)]/LNCOG 



(29) 



DRCOSY = [LCOGY - JY(inboard)]/LNCOG 



(30) 



DRCOSZ = [LCOGZ - JZ(inboard)]/LNCOG 



(31) 



where LCOGX is the X location of the CG with respect to the inertia 
frame; JX is the X location of the inboard joint location with respect to 
the inertia; and LNCOG is magnitude of the length from the inboard 
joint to the CG. 

Once the directional cosines were known, the same method as 
used by previous investigators was used to calculate the MATA and 
MATB matrices. 

C. CONSTRAINTS 

With the desire to simulate the motion of an actual robotic manip- 
ulator and to compare the simulation with data from the manipulator, 
constraints had to be added into the simulation so the simulation 
would take into account the the joint mechanical restrictions. In the- 
ory, each joint is a one degree of freedom connection. However, in the 
chosen arm, the zero joint rotates in the Z direction only, while joints 
one and two rotate about both the X and Y axes (Figure 3). The previ- 
ously mentioned constraints led to the following equations: 
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Figure 3. Robot Axis Rotation with Constraints 
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AX1 = 0 



(32) 



AY1 = 0 (33) 

AZ1 = 0 (34) 

WDX1 = 0 (35) 

WDYl = 0 (36) 

WDZ1 = WDZ2 = WDZ3 (37) 



The concept for entering the constraints into the simulation was 
developed in Reference 3 and led to the constraint section of the DSL 
code as seen in Appendix A and Appendix B. The basic concept which 
was developed was to zero out the row only in MATA, then set the 
diagonal of that row to one and the corresponding row of MATB to 
zero. This concept was changed to include zeroing out the row and 
column of MATA corresponding to the row of vector x, which was to 
be zero, along with setting the diagonal of the row to 1 and the corre- 
sponding row of MATB to zero. 

D. GRAVITATIONAL TORQUES 

The simulation of gravitational effects on the arm required that an 
equalizing torque be applied at the joints to overcome the gravitational 
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D. GRAVITATIONAL TORQUES 

The simulation of gravitational effects on the arm required that an 
equalizing torque be applied at the joints to overcome the gravitational 
pull (Figure 4). This torque varied as the Z location of the center of 
gravity of links 2 and 3 varied because the magnitude of the moment 
arm varied as the link moved. The lever arm which affected the gravi- 
tational torque was the projection of the vector from the inboard joint 
to the link center of gravity onto the XY plane (Figure 5). Knowing this 
lever arm and the weight of the links, the following equations were 
used to calculate the equilibrium torque needed to overcome gravity: 

MARM2A = SQRT (LCOGX2*LCOGX2 + LCOGY2*LCOGY2) (38) 
MARM2B = SQRT (LCOGX3*LCOGX3 + LCOGY3*LCOGY3) (39) 
MARM3 = SQRT ((LCOGX3-JX2)**2 + (LCOGY3^JY2)**2) (40) 

TGI = MAR2A * WG2 + MARM2B * WG3 (41) 

TG2 = MARM3 * WG3 (42) 

The equations above represent the magnitude of the required 
torque which must be resolved into its X and Y components (Figure 6). 
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EQUALIZING TORQUES 




Figure 4. Gravitational Torques 




Figure 5. Gravitational Torque Moment Arms 




Figure 6. Gravitational Torque X and Y Components 
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With the given constraints, this was done in the following manner. 
Angle THZ was calculated from the second integral of WDZ1 (the base 
link only revolved around its vertical axis). Then its cosine and sine 
were used to obtain the X and Y components of the gravitational equi- 
librium torque as follows: 

TGX = TG * COS (THZ) (43) 

TGY = TG * SIN (THZ) (44) 



E. INERTIAS 

The inertias of the links of the robot were calculated the first 
time from the equations of Reference 2 in the global (inertial) 
reference frame and then transferred to the local coordinates (Figure 
7) by the use of a transformation [Ref. 9], giving the following equation: 

Inertia(local) = TMAT * Inertia(global) * TMATTR (45) 

where MATA is a transformation matrix based on the link angles 
[Ref. 1 and Ref. 10]. Knowing that the local inertias remain constant, 
the global inertias for each link were calculated for each time iteration 
by the use of the inverse transformation below: 

Inertia(global) = TMATTR * Inertia(local) * TMAT (46) 
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Figure 7. Inertia Coordinate Systems 



20 



IV. SIMULATION NONSINGULAR MOTION VALIDATION 



A. TWO-DIMENSIONAL MOTION VALIDATION 

A known motion which had adjacent links aligned was that of the 
two-dimensional double pendulum (Figure 8). A comparison of the 
simulation to the theoretical solution was used to validate the 
nonsingularity, the motion tracking, and the gravitational effects of the 
simulation. 

The theoretical solution [Ref. 1 1] used a Lagrange equation which 
only included the linear kinetic energy of the system (equation 47), 
while the simulation included both the linear and rotational kinetic 
energies (equation 48). To account for the difference, the inertia in 
equation 48 was set to a small value. Note that it was not possible to 
set the inertias to zero because the inertia entries in matrix MATA 
were on the diagonal and would thus not have allowed the inverse to 
be calculated. 



Kinetic Energy = 1/2 * m * v 2 (47) 

Kinetic Energy = l/2*m*v 2 + 1/2 1* w 2 (48) 

The point mass method developed in Reference 3 allowed the 
first mass of the double pendulum to be set to a value equal to that of 
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Figure 8. Double Pendulum Configuration 
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the outer point mass of link 2 plus the inner point mass of link 3 
(equation 49). The second mass of the double pendulum was set to the 
value of the outer point mass of link 3 (equation 50). 



ml (dp) = m (2,2) + m (3,1) 


(49) 


m2 (dp) = m (3.2) 


(50) 



The lengths of the double pendulum were the same as the link 
lengths (Figure 8). 

The results of the theoretical and simulation program for the 
double pendulum problem agreed (Figures 9 and 10), and would have 
been identical had the inertia been set to zero. Tracking of the motion 
through the potential singular points presented no difficulties to the 
simulation (Figure 11). 

B. THREE-DIMENSIONAL NONSINGULAR MOTION 

A motion which had potential singular points was chosen which 
validated the nonsingularity of the simulation in three dimensions 
(Figure 12). The motion which was chosen was to input the joint 1 and 
joint 2 angles as a time function and allow the third joint angle to 
remain zero, so links 2 and 3 were always aligned and potentially 
singular. This chosen motion also had all three links aligned at time 
equal to 0.785 seconds (Figure 12). 
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Figure 9. Double Pendulum Angle 2 
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Figure 10. Double Pendulum Angle 3 
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Figure 11. Double Pendulum Simulation Through 
Potential Singular Points (angle = 0) 
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Figure 12. Three-Dimensional Nonsingular Validation 
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The simulation again had no difficulties tracking the input motion 
through the potentially singular points (Figure 13). The error in the 
predicted and simulated end effector position (Figure 14) was never 
greater than 1.0 percent. 
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Figure 13. Three-Dimensional Potential Singular Points 
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Figure 14. End Effector Position Error 
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V. EXPERIMENTAL VALIDATION 



A. MANIPULATOR 

A robot manipulator was chosen to check the agreement of the 
simulation to data from an actual robot arm. The arm that was available 
was the NEPTUNE II (Figure 15), which is a low-performance, 
hydraulic-operated arm. This arm required certain hardware modifi- 
cations, namely: 

• the remounting of the electronic solenoid boards to facilitate 
access and provide protection from hydraulic leaks; 

• installation of bleed holes in the major joint pistons to allow air to 
be bled off when the system has been opened for repairs; 

• design and manufacture of a pump and accumulator stand that 
would provide spray protection for the manipulator in the case of 
malfunctions; 

• modification to the plumbing of the pump and accumulator to 
include an electrically controlled solenoid valve operated from the 
Electronic Control Panel (ECP); 

• rewiring the pump to be electrically controlled from the ECP; 

• redesign and manufacture of the waist joint (joint 0) shaft and 
coupling, along with that of several piston end plates; and 

• an overhaul of the complete electronics. 

B. PRESSURE TRANSDUCERS 

Pressure transducers which had previously been installed on the 
NEPTUNE II arm at joints 1 and 2 (Figure 16) allowed for data collec- 
tion to validate the two-dimensional simulation with gravitational 
effects. Initially, the pressure transducers had to be calibrated. 
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Figure 15. NEPTUNE II Robot 
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Figure 16. Robot Pressure Transducers 
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A 3,000-pound nitrogen bottle with a 200 psi regulator was used 
for pressure transducer calibration. This was connected to a test rig 
which consisted of a varlinear pressure adjustment and a 200-pound 
pressure gage which had been calibrated and was incremented in 0.2- 
pound increments (Figure 17). The varlinear allowed the pressure at 
the transducer to be varied in fine increments from zero to 120 psi, 
which was the stated range of the transducers. The electrical connec- 
tions from the transducers had been hard-wired to the ECP and then 
to a digital multimeter (Fluke 8024 B); the meter 20-volt scale was 
used to read the voltage output of the transducers (the voltage was 
read to the second decimal place). Each Omega Series PX302 pressure 
transducer had two complete sets of data from 0 to 120 psi; the aver- 
ages of these are provided in Table 1. The pressure transducers were 
very linear from 0 to 115 psi, with each 5 psi producing a voltage of 
0.5 volts (Figure 18). This range of pressures was well acceptable for 
the NEPTUNE II, which operates between 0 and 118 psi (Ref. 12]. 
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Figure 17. Pressure Transducer Calibration Set-Up 
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TABLE 1 



PRESSURE TRANSDUCER DATA 



PI 


P2 


P3 


P4 


volts 


volts 


volts 


volts 


-0.010 


0.000 


0.015 


0.000 


0.500 


0.510 


0.520 


0.500 


0.995 


1.005 


1.025 


1.005 


1.500 


1.500 


1.530 


1.500 


1.985 


1.995 


2.020 


2.000 


2.500 


2.500 


2.530 


2.500 


3.000 


3.000 


3.030 


3.000 


3.490 


3.500 


3.530 


3.490 


3.990 


3.995 


4.030 


3.995 


4.500 


4.500 


4.530 


4.500 


4.995 


4.995 


5.025 


4.990 


5.490 


5.500 


5.520 


5.490 


5.995 


5.995 


6.020 


5.990 


6.490 


6.500 


6.520 


6.490 


6.990 


7.000 


7.020 


6.990 


7.490 


7.500 


7.510 


7.500 


7.990 


8.005 


8.010 


8.000 


8.490 


8.510 


8.510 


8.500 


8.990 


9.005 


9.000 


9.000 


9.480 


9.500 


9.500 


9.500 


9.990 


10.005 


10.000 


10.000 


10.490 


10.510 


10.500 


10.500 


10.990 


11.010 


11.000 


11.000 


11.485 


11.490 


11.490 


11.500 


11.580 


11.490 


11.990 


12.000 
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Figure 18. Pressure Transducer Data 
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C. MEASUREMENTS 
1. Link Lengths 

With the necessity to disassemble the NEPTUNE II for 
repairs, the link length measurements were physically measured. 
These measurements are provided in Table 2 to the nearest 1/8 inch. 

TABLE 2 

LINK LENGTHS 



Parameter 


Length 


Length 




inch 


meter 


LINKL1 


10.250 


0.2604 


LINKL2 


16.375 


0.4159 


LINKL3 


13.750 


0.3498 



2. Link Masses 

Also during the disassembly of the robot, the link masses 
were measured using a scale which was in half-pound increments. The 
design of the robot allowed for a good approximation of the two 
masses per link as used in the simulation (Figures 19 and 20). This 
was a crude measurement, but it was felt that this was sufficient to be 
used for the simulation and that the time necessary to completely dis- 
assemble the robot in order to be able to obtain closer measurements 
was not justified. The masses which were measured are provided in 
Table 3. 
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Figure 19. Robot Link Masses 
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Figure 20. Simulation Link Masses 
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TABLE 3 



LINK 


MASSES 




Parameter 


Pounds 


Kilograms 


MASS(l.l) est. 


5 


2.273 


MASS(1,2) est. 


15 


6.818 


Ml (measured) 


20 


9.091 


MASS(2, 1) est. 


1 


0.455 


MASS (2, 2) est. 


1 


0.455 


M2 (measured) 


2 


0.910 


MASS(3, 1) est. 


13 


5.909 


MASS(3,2) est. 


13 


5.909 


M3 (measured) 


26 


11.818 



3. Length from COG of Link to Center of Mass 

The point mass centers were assumed to be at the joints 
(Figure 20). A simple calculation of the moments around the center of 
gravity was performed; this provided the data in Table 4. 

TABLE 4 

LENGTH FROM LINK CENTER OF GRAVITY 
TO MASS CENTER OF GRAVITY 



Parameter 


Length 

inch 


Length 

meter 


L(l.l) 


7.6875 


0.1953 


L(l,2) 


2.5625 


0.0651 


L(2,l) 


8.1875 


0.2080 


L(2,2) 


8.1875 


0.2080 


L(3,l) 


6.8750 


0.1746 


L(3,2) 


6.8750 


0.1746 
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D. DATA COLLECTION 

Position data and pressure data were taken for several movements 
of links 2 and 3 of the robot from the electronic control panel and 
recorded on a four-channel strip chart recorder. An example of the 
recorded data is provided (Figure 21). 

E. DATA REDUCTION 

The data taken for the various runs was reduced using the follow- 
ing equation to obtain joint torques. 

TORQUE = A PRESSURE * AREA(piston) * RADIUS(lever arm) (51) 

where AREA(piston) = 0.00312 square meters and RADIUS(lever arm) 
= 0.0597/2 meters. 

F. VALIDATION 

The delta pressure data versus time was entered into the robot 
validation program as a time function and the torque was calculated as 
a function of time from this data. The simulation position results 
agreed with the actual robot position in the general trend of motion 
(Figure 22). It was felt that the inaccuracies in the results were due to 
both the simulation round-off errors and the crude methods used in 
the collection of the robot link masses and link lengths, particularly 
the lengths from the center of gravity of the link to the center of 
gravity of the point masses. The scalloped shape of the simulated 
result was attributed to the normalization of the directional cosines. 
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Figure 21. Pressure Recordings 
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Figure 22. Robot/Simulation Motion 
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VI. RESULTS AND RECOMMENDATIONS 



1. A nonsingular dynamic simulation of a rigid revolute three-link 

manipulator has been further developed. It includes: 

• Gravity, 

• Three-Dimensional Motion, 

• Comparatively straightforward dynamics. 

2. Accuracy and nonsingularity has been validated with a double 

pendulum. 

3. Three-dimensional nonsingularity has been validated. 

4. Procedures for actual robot data collection have been developed. 

5. The following recommendations are provided: 

• Simulate actual three-dimensional robot performance and 
compare predicted to measured variables. 

• Enhance the computer program code to enable it to be more 
interactive. That is, let the user specify the constraints during 
execution. 

• Use this simulation to develop advanced controllers for rigid 
robot linkages. 



45 



APPENDIX A 



DIRECT DYNAMIC SIMULATION PROGRAM 



TITLE DIRECT DYNAMICS PROBLEM 



************ ************************************************************* 



* 

* 



* 



** 



THIS IS A PROGRAM THAT WILL SOLVE THE DIRECT DYNAMICS PROBLEM FOR A * 
3 LINK RIGID BODY MANIPULATOR WITHOUT THE USE OF ANY TRANSFORMATION * 
MATRICES. THE ORIGINAL PROGRAM WAS WRITTEN BY LT ATINOK AND HAS * 
HAS BEEN GREATLY MODIFIED TO INCLUDE 3 D MOTION AND GRAVITATIONAL * 
EFFECTS BY LT R. M. VERBOS USN SEPTEMBER 1988 * 



» J- Jtf J#JL»U •'# J# J# J# J# J# J# J# Jt« J# 

4 \ 4 \ 4 \ 4% 4 \ 4 \ 4 \ 4\ f\4\ # V / i 4 \ 4\ 4<4\ 4\ 4\ 4 \ 4\ 4\ 4% 4% 4% 4% 4\ 4 \ 4 % 4\ 4 \ 4 \ 4% 4 \ 4 \ 4\ 4 i 4\ 4\ 4\ 4 \ 4% 4% 4\ 4\ 4\ 4i 4\ 4\ 4% 4 \ 4% 4 \ 4 \ 4 \ 4 % 4 > 4\ 4\ 4\ 4 % 4\ 4 \ 4 \ 4 \ 4 \ 4 i 4\ 4\ 4\ 4\ 



TERMINAL 
METHOD RKSFX 

PRINT . 10 ,DRCANX( 1) , DRCANY ( 1 ) ,DRCANZ(1) ,. . . 

DRCANX( 2) ,DRCANY( 2) ,DRCANZ(2) ,DRCANX(3) ,DRCANY(3) ,DRCANZ(3) . . 

DRCSX( 1) ,DRCSY( 1) ,DRCSZ( 1) ,DRCSX( 2) ,DRCSY(2) ,DRCSZ(2) ,DRCSX(3) ,. . . 

DRCSY( 3) ,DRCSZ(3) , . . . 

JX0,JY0,JZ0,JX1,JY1,JZ1,JX2,JY2,JZ2,. . . 

LCOGX( 1 ) , LCOGY( 1 ) , LCOGZ( 1 ) , LCOGX( 2) , LCOGY( 2 ) , LCOGZ( 2 ) , . . . 

LCOGX( 3) , LCOGY ( 3 ) , LCOGZ( 3 ) , . . . 

TOX ,T0Y ,T0Z ,T1X,T1Y,T1Z ,T2X,T2Y,T2Z , . . . 

WDX( 1) ,WDY( 1) ,WDZ( 1) ,WDX( 2) ,WDY(2) ,WDZ(2) ,WDX(3) ,WDY(3) ,WDZ(3) ,. . . 

AX1 , AY1 , AZ1 , AX2 , AY2 , AZ2 , AX3 , AY3 , AZ3 , . . . 

W1X , VI Y , W 1 Z , W2X , W2Y , W2Z , W3X , W3Y , W3 Z , . . . 

FXO ,FY0 ,FZ0 ,FX1 , FY1 ,FZ1 ,FX2 , FY2 ,FZ2 , . . . 

IXXT(l) ,IYYT(1) ,IZZT(1),IXXT(2) ,IYYT(2) ,IZZT(2) ,IXXT(3) ,IYYT(3),. . . 

I ZZT( 3 ) , IXYT( 1 ) , I YZT( 1 ) , IXZT( 1 ) , IXYT( 2 ) , I YZT( 2) , IXZT( 2) , IXYTC 3 ) , . . . 

IYZT( 3) , IXZT( 3) . . 

LNCOG 1 , LNCOG2 , LNCOG3 , THZANG , COSTHZ , S INTHZ , TIPX , TIPY , TIPZ , . . . 

ANG12X , ANG12Y, ANG12Z , ANG23X, ANG23Y, ANG23Z , . . . 

T1CH.T2CH, IER 

CONTROL FINTIM =1.5, DELT = .0005, DELMAX = .1, DELPRT = .10 
D DIMENSION MATA(27,27) ,MASS( 3 , 2) , L( 3 , 2) ,RX( 3 , 2) , RY( 3 , 2) ,RZ(3,2) 

D DIMENSION IXX( 3 , 2) , IXZ( 3 , 2) , IXY( 3 ,2) , IYY( 3 ,2) , IYZ( 3 ,2) , IZZ( 3 , 2) 

D DIMENSION IMAT(3,3,3) ,NIMAT(3,3) ,TMAT(3,3) ,TMATTR(3,3) ,MATTMP(3,3) 

D DIMENSION LIMAT(3,3,3) v 

FIXED IER,I,J,M,K,P,N,IA, IDGT, A, I 1 

ARRAY MATB(27) ,ABMATB(27) ,LCOGX(3) ,LCOGY(3) ,LCOGZ(3) 

ARRAY VECTA0<3) ,VECTB0(3) ,VECTA1(3) ,VECTB1(3) ,VECTA2(3) ,VECTB2(3) 

ARRAY VECTA(3) ,VECTB(3) 

ARRAY WDX( 3) ,WDY( 3) ,WDZ(3) ,W1(3) ,W2(3) ,W3(3) 

ARRAY RBG1( 3) ,RAG1(3) ,RBG2(3) ,RAG2(3) ,RBG3(3) 

ARRAY WKAREA( 850) 

ARRAY IXXTC 3) , IYYT( 3) , IZZT( 3) , IXYT( 3) , IXZT( 3) , IYZT( 3) 
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ARRAY DRCANX( 3 ) ,DRCANY( 3) ,DRCANZ( 3) 
ARRAY DRCSX( 3) ,DRCSY( 3) ,DRCSZ( 3) 

INITIAL 



***** INPUT ***** 

* INPUT PARAMETER CONSTANTS 

55 A = 15. 0D0 

P = 0.0D0 
W = 2 * PI 

IDGT = 6 
G = 9. 8 IDO 
N = 27 
M = 1 
IA = 27 
IER = 0 
LEVELQ = 0 
LEVLDQ = 0 

* INPUT JOINT LOCATIONS IN METERS 

JXO = 0. ODO 
JYO = 0. ODO 
JZO = 0. ODO 
JX1 = 0. ODO 
JY1 = 0. ODO 
JZ1 = 0. 2D0 
JX2 = 0. ODO 
JY2 = 0.416D0 
JZ2 = 0.2D0 



INPUT TORQUE CONSTANTS 



TOX 


= 


0. ODO 


TOY 


= 


0. ODO 


TOZ 


= 


0. ODO 


T1X 


= 


0. ODO 


T1Y 


= 


0. ODO 


T1Z 


= 


0. ODO 


T2X 


= 


0. ODO 


T2Y 


= 


0. ODO 


T2Z 


= 


0. ODO 


TGI 


= 


0. ODO 


TG2 


— 


0. ODO 


T1FNC 


= 


C. ODO 


T2FNC 


= 


0. ODO 



INPUT DISTANCE FROM CENTER OF LINK TO CENTER OF MASS 
FOR EACH LINK ENDS 
L(l,l) = 0.05D0 
L( 1,2) = 0. 15D0 
L( 2 , 1) = 0. 20D0 
L( 2 , 2) = 0. 216D0 
L(3,l) = 0. 225D0 
L(3,2) = 0.226D0 



* INPUT THE LINK LENGTHS OF THE ROBOT 
LINKL1 = 0.20D0 
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LINKL2 = 0.416D0 
LINKL3 = 0.45 IDO 



110 



* 

120 



INPUT MASS AT LINK ENDS IN KILOGRAMS 
MASS( 1,1) = 11. ODO 
MASS( 1,2) = 33. ODO 
MASS( 2,1) = 2. 2D0 
MASS( 2,2) = 2. 2D0 
MASS ( 3 , 1) = 28. 6D0 
MASS( 3,2) = 28.6D0 

INPUT LOCATION OF LINK CENTERS OF GRAVITY 



LCOGX( 1) 




0. ODO 


XI 


= 


LCOGX(l) 


LCOGY( 1) 


— 


0. ODO 


Y1 


= 


LCOGY(l) 


LCOGZ( 1) 


= 


0. 10D0 


Z1 


= 


LCOGZ(l) 


LCOGX( 2 ) 


— 


0. ODO 


X2 


= 


LC0GX(2) 


LCOGY( 2) 


= 


0. 208D0 


Y2 


= 


LCOGY( 2) 


LCOGZ( 2) 




0. 20D0 


Z2 


= 


LC0GZ(2) 


LCOGX( 3) 




0. ODO 


X3 


— 


LCOGX( 3) 


LCOGY( 3) 


= 


0. 6415D0 


Y3 


~ 


LCOGY( 3) 


LCOGZ( 3) 


— 


0. 2D0 


Z3 




LCOGZ( 3) 



* INPUT MASS OF EACH LINK IN KG 

Ml = 44. ODO 
M2 = 4.4D0 
M3 = 57.2D0 

* INPUT ACCELERATIONS OF JOINT ZERO 

AXO = 0. ODO 
AYO = 0. ODO 
AZO = 0. ODO 

* INPUT THE INITIAL DIRECTION COSINES 

DRCSX(l) = 0. ODO 
DRCSY(l) = 0. ODO 
DRCSZ( 1) = 1. ODO 
DRCSX(2) = 0. ODO 
DRCSY( 2) = 1. ODO 
DRCSZ( 2) = 0. ODO 
DRCSX(3) = 0. ODO 
DRCSY( 3) = 1. ODO 
DRCSZ( 3) = 0. ODO 

* INPUT THE INITIAL DIRECTION COSINE ANGLES 

DRCANX(l) = 90. ODO 
DRCANY(l) = 90. ODO 
DRCANZ(l) = 0. ODO 
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DRCANX( 2) = 90. ODO 
DRCANY( 2) = 0. ODO 

DRCANZ( 2) = 90. ODO 
DRCANX( 3) = 90. ODO 
DRCANY( 3) = 0. ODO 

DRCANZ( 3) = 90. ODO 






160 



170 



INITIALIZE ***** 
OMEGA AND OMEGA DOT 
DO 170 I = 1,3 



W1(I) 


= 0. ODO 


W2( I) 


= 0. ODO 


W3( I) 


= 0. ODO 


WDX(I) 


= 0. ODO 


WDY(I) 


= 0. ODO 


WDZ(I) 


= 0. ODO 



CONTINUE 



THZ = o.oro 



* INITIALIZE MATRIX A AND B TO ZERO 
DO 180 I = 1,27 
DO 175 J = 1,27 

MATA( I , J) = 0. ODO 
175 CONTINUE 

MATB(I) = 0. ODO 
180 CONTINUE 



***** CALCULATIONS ***** 
* WEIGHTS (NEWTONS) 

185 WG1 = M 1*G 

WG2 = M2*G 
WG3 = M3*G 



* COMPUTE THE LENGTH FROM THE INBOARD JOINT TO COG 

LNC0G1 = DSQRT ( LCOGX( 1)*LC0GX( 1) + LCOGY( 1)*LC0GY( 1 ) +. . . 

LCOGZ( 1)*LC0GZ( 1 ) ) 

LX 2 = LCOGX( 2) - JX1 
LY2 = LC0GY(2) - JY1 
LZ2 = LCOGZ( 2) - JZ1 

LNC0G2 = DSQRT ( LX2*LX2 + LY2*LY2 + LZ2*LZ2 ) 

LX 3 = LCOGX( 3) - JX2 
LY3 = LCOGY( 3) - JY2 
LZ3 = LCOGZ( 3) - JZ2 

LNC0G3 = DSQRT ( LX3*LX3 + LY3*LY3 + LZ3*LZ3 ) 

* COMPUTE INITIAL INERTIAS BASED ON POINT MASSES 

* IN GLOBAL COORDINATES 

190 DO 225 I = 1,3 

RX( 1,1) = -L( 1,1) * DRCSX(I) 

RX( 1,2) = L( I , 2) * DRCSX(I) 

RY( I , 1) = -L( 1,1) * DRCSY(I) 

RY( I , 2) = L( I , 2) * DRCSY(I) 

RZ( 1,1) = -L( 1,1) * DRCSZ(I) 

RZ( 1,2) = L( I , 2) * DRCSZ(I) 

200 IXX(I,1) = MASS( 1,1) * ( (RY( 1,1) * RY(I,1)) + (RZ( 1,1) * RZ(I,1))) 
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205 



225 



IXX( 1,2) = MASS( I 
IXXT(I) = IXXCI, 
IYY( 1,1) = MASS( I 
IYY( 1,2) = MASS( I 
IYYT(I) = IYY(I, 
IZZCI.l) = MASS( I 
IZZ( I , 2) = MASS( I 
IZZT(I) = IZZ(I, 
IXY( 1,1) = HASS( I 
IXY( 1,2) = MASS( I 
IXYT(I) = IXY( I , 
IXZ(I,1) = MASS( I 
IXZ( 1,2) = MASS( I 
IXZT(I) = IXZ(I, 
IYZ( I , 1 ) = MASS(I 
IYZ( 1,2) = MASS( I 
IYZT(I) = IYZ( I , 
IF (IXXT(I) .LE. 
IXXT(I) = .02 
ELSE 

IXXT(I) = IXXT(I) 
END IF 

IF (IYYT(I) .LE. 
IYYT(I) = .02 
ELSE 

IYYT(I) = IYYT( I 
END IF 

IF (IZZT(I) .LE. 
IZZT(I) = .02 
ELSE 

IZZT(I) = IZZT(I 
END IF 

IMAT( 1,1,3) 
IMAT( 1 , 1 > 2) 
IMAT( 1,1)3) 
IMAT( 1,2,1) 
IMAT( 1,2,2) 
IMAT(I,2,3) 
IMAT( 1,3,1) 
IMAT( I ,3 >2) 
IMAT( 1,3,3) 
CONTINUE 



,2) * ( (RY( 1,2) 
1) + IXX( 1,2) 
,1) * ( (RX( 1,1) 
,2) * ( (RX( 1,2) 
1) + IYY( 1,2) 
,1) * C (RXC I , 1) 
,2) * C C RX( 1,2) 
1) + IZZ(I,2) 
,1) * RX(I,1) * 
,2) * RX( 1,2) * 
1) + IXY( 1,2) 

, 1) * RZ(I,1) * 
,2) * RZ(I,2) * 
1) + IXZ( 1,2) 

, 1) * RYd > 1) * 
,2) * RY(I, 2) * 
1) + IYZ( 1,2) 

. 02) THEN 



.02) THEN 



) 

. 02) THEN 



) 



IXXT(I) 

IXYT(I) 

IXZT(I) 

■IXYT(I) 

IYYT(I) 

IYZT(I) 

■IXZT(I) 

■IYZT(I) 

IZZT(I) 



* RY(1, 2)) + C RZ ( 1,2) 

* RX(I,1)) + (RZ( 1,1) 

* RX( 1,2)) + ( RZ ( 1,2) 

* RX( 1,1)) + (RY(I,1) 

* RXC I, 2)) + (RY(I, 2) 

RY(I,1) 

RY ( I , 2 ) 

RX( 1,1) 

RX(I,2) 

RZ( I , 1) 

RZ( I , 2) 



* RZ( 1,2))) 

* RZ( 1,1))) 

* RZ( I , 2) ) ) 

* RY ( I , 1 ) ) ) 

* RY(I, 2))) 



DUE TO LINK 1 CONSTRAINTS LINK 1 INERTIAS ARE CONSTANT 
IXXT(l) = IMAT( 1,1,1) 

IXYT(l) = IMAT( 1,1,2) 

IXZT(l) = IMATC 1,1,3) 

IYYT(l) = IMAT( 1,2,2) 

IYZT(l) = IMAT( 1,2,3) 

IZZT(l) = IMAT( 1,3,3) 



TRANSFORM THE INITIAL INERTIAS TO LOCAL COORDINATED 
DO 9 1=2, 3 

TMAT( 2,1) = -DCOS ( THZ ) 

TMAT( 2,2) = -DSIN ( THZ ) 

TMAT( 2,3) = 0. ODO 
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TMAT( 3,1) = DRCSX(I) 

TMAT( 3,2) = DRCSY(I) 

TMAT( 3,3) = DRCSZ(I) 

VECTA(l) = TMAT( 2,1) 

VECTA( 2) = TMAT( 2,2) 

VECTA( 3) = TMAT( 2,3) 

VECTB(l) = TMAT(3, 1) 

VECTB( 2) = TMAT( 3 , 2) 

VECTB( 3) = TMAT( 3,3) 

CALL CPROD ( VECTA , VECTB , MI 1 , M J1 , MK1 ) 

TMAT( 1,1) = Mil 
TMAT( 1,2) = MJ1 
TMAT( 1,3) = MK1 
TMATTR( 1,1) = TMAT( 1,1) 

TMATTR( 1,2) = TMAT(2,1) 

TMATTR( 1,3) = TMAT(3,1) 

TMATTR( 2,1) = TMAT(1,2) 

TMATTR( 2,2) = TMAT(2,2) 

TMATTR( 2,3) = TMAT(3,2) 

TMATTR(3, 1) = TMAT(1,3) 

TMATTR( 3,2) = TMAT(2,3) 

TMATTR(3,3) = TMAT(3,3) 

DO 5 II = 1,3 
DO 5 J =1,3 
TEMP = 0. ODO 
DO 1 K =1,3 

TEMP = TMAT(I1,K) * IMAT(I,K,J) + TEMP 
1 CONTINUE 

MATTMP(I1, J) = TEMP 
5 CONTINUE 

DO 8 II = 1,3 

DO 8 J = 1,3 

TEMP = 0. ODO 
DO 7 K =1,3 

TEMP = MATTMP( II ,K) * TMATTR(K, J) + TEMP 

7 CONTINUE 

LIMAT(I . II, J) = TEMP 

8 CONTINUE 

9 CONTINUE 

DERIVATIVE 

NOSORT 

230 CALL ERRSET (208,256,-1,1,1) 

CALL UERSET( LEVELQ , LEVLDQ ) 

* INITIALIZE MATRIX A AND B TO ZERO 
DO 240 I = 1,27 
DO 235 J = 1,27 

MATA( I , J) = 0. ODO 
235 CONTINUE 

MATB(I) = 0. ODO 
240 CONTINUE 



INPUT JOINT EQUATIONS 
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JOINT ZERO EQUATIONS 
W1 X (W1 X RB/G1) 

RBG1(1) = JXO - LCOGX(l) 

RBG1( 2) = JYO - LCOGY(l) 

RBG1( 3) = JZO - LCOGZ(l) 

VECTA(l) = Wl( 1) 

VECTA( 2) = Wl( 2) 

VECTA( 3) = Wl( 3) 

CALL CPROD( VECTAO , RBG 1 , MICO , MJCO , MKCO) 
VECTBO(l) = MICO 
VECTBO( 2) = MJCO 
VECTBO( 3) = MKCO 

CALL CPROD( VECTAO , VECTBO , MICO , MJCO , MKCO ) 

INPUT JOINT EQUATIONS 
JOINT ZERO EQUATIONS 
W1 X (W1 X RB/G1) 

RBG1(1) = JXO - LCOGX(l) 

RBG1( 2) = JYO - LCOGY(l) 

RBG1C3) = JZO - LCOGZ(l) 

VECTA( 1) = Wl( 1) 

VECTA( 2) = Wl(2) 

VECTA( 3) = Wl( 3) 

CALL CPROD( VECTA, RBG 1 , MICO , MJCO , MKCO) 
VECTB(l) = MICO 
VECTB( 2) = MJCO 
VECTB( 3) = MKCO 

CALL CPROD( VECTA , VECTB , MICO , MJCO ,MKCO ) 
W1 X (W1 X RA/G1) 

RAG1(1) = JX1 - LCOGX(l) 

RAG1( 2) = JY1 - LCOGY(l) 

RAG1( 3) = JZ1 - LCOGZ(l) 

VECTA(l) = W 1 ( 1 ) 

VECTA( 2) = Wl( 2) 

VECTA( 3) = Wl( 3) 

CALL CPROD (VECTA, RAG 1 ,MIC1 ,MJC1 ,MKC1) 
VECTB ( 1) = MIC1 
VECTB( 2) = MJC1 
VECTB( 3) = MKC 1 

CALL CPROD (VECTA, VECTB, MIC1 ,MJC1 ,MKC1) 
W2 X (W2 X RB/G2) 

RBG2( 1) = JX1 - LCOGX( 2) 

RBG2(2) = JY1 - LCOGY( 2) 

RBG2( 3) = JZ1 - LCOGZ( 2) 

VECTA(l) = W2( 1) 

VECTA( 2) = W2( 2) 

VECTA( 3) = W2( 3) 

CALL CPROD ( VECTA, RBG2 ,MIC2 ,MJC2 ,MKC2) 
VECTB ( 1) = MIC2 
VECTBf 2) = MJC2 
VECTB( 3) = MKC 2 

CALL CPROD (VECTA, VECTB, MIC 2, MJC2,MKC2) 
W2 X ( W2 X RA/G2) 

RAG2( 1) = JX2 - LCOGX( 2) 

RAG2( 2) = JY2 - LCOGY(2) 



RAG2( 3) = JZ2 - LCOGZ( 2) 

VECTA(l) = W2( 1) 

VECTA( 2) = W2( 2) 

VECTA( 3) = W2( 3 ) 

CALL CPROD ( VECTA, RAG2 , MIC3 , MJC3 , MKC3) 
VECTB(l) = HIC3 
VECTB( 2) = MJC3 
VECTB( 3) = MKC3 

CALL CPROD( VECTA , VECTB , MIC3 , MJC3 , MKC3 ) 
W3 X (W3 X RB/G3) 

RBG3( 1) = JX2 - LCOGX( 3) 

RBG3( 2) = JY2 - LCOGY(3) 

RBG3( 3) = JZ2 - LCOGZ( 3) 

VECTA(l) = W3( 1) 

VECTA( 2) = W3( 2) 

VECTA( 3) = W3( 3) 

CALL CPROD ( VECTA , RBG3 , MIC4 , MJC4 , MKC4 ) 
VECTB ( 1) = MIC4 
VECTB(2) = MJC4 
VECTBC3) = MKC4 

CALL CPROD (VECTA, VECTB, MI C4,MJC4,MKC4) 

INERTIA TRANSFORMATION 
DO 390 I = 2, 3 

TMAT( 2,1) = -DCOS ( THZ ) 

TMAT( 2,2) = -DSIN ( THZ ) 

TMAT( 2,3) = 0. 0D0 
TMAT( 3,1) = DRCSX(I) 

TMATC 3,2) = DRCSY(I) 

TMAT (3,3) = DRCSZ(I) 

VECTA(l) = TMAT( 2,1) 

VECTA( 2) = TMAT( 2,2) 

VECTA( 3) = TMAT(2,3) 

VECTB ( 1) = TMAT( 3,1) 

VECTB( 2) = TMAT( 3,2) 

VECTB( 3) = TMAT( 3,3) 

CALL CPROD (VECTA, VECTB, Mil, MJ1,MK1) 

TMAT( 1,1) = Mil 

TMAT( 1,2) = MJ1 

TMAT( 1,3) = MK1 

TMATTR( 1,1) = TMAT( 1,1) 

TMATTR( 1,2) = TMAT(2,1) 

TMATTR( 1,3) = TMAT(3,1) 

TMATTR( 2,1) = TMAT(1,2) 

TMATTR( 2,2) = TMAT(2,2) 

TMATTR(2,3) = TMAT(3,2) 

TMATTR( 3,1) = TMAT(1,3) 

TMATTR( 3,2) = TMAT(2,3) 

TMATTR(3,3) = TMAT(3,3) 

DO 375 II = 1,3 
DO 375 J = 1,3 
TEMP = 0. 0D0 
DO 370 K = 1,3 
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TEMP = TMATTR( I 1 ,K) * LIMAT(I,K, 
370 CONTINUE 

MATTMP( II , J) = TEMP 
375 CONTINUE 



DO 380 II = 1,3 
DO 380 J = 1,3 
TEMP = 0. 0D0 
DO 378 K = 1,3 

TEMP = MATTMP( II ,K) * TMAT(K, J) + 
378 CONTINUE 

NIMAT( II, J) = TEMP 
380 CONTINUE 



IXXT(I) = NIMAT( 1,1) 
IXYT(I) = NIMAT( 1,2) 
IXZT(I) = NIMAT( 1,3) 
IYYT(I) = NIMAT( 2,2) 
IYZT(I) = NIMAT( 2,3) 
IZZT(I) = NIMAT( 3,3) 

390 CONTINUE 



VoWoWc ENTER CONSTANTS INTO MATRIX A AND B 



LINK ONE 



■;V 


SUM OF FORCES 


* 


X 


DIRECTION 




395 




MATA( 1,1) 


= 1.0D0 






MATA( 1,4) 


= Ml 






MATA( 1,10) 


= -1.0D0 






MATB( 1) 


= 0. 0D0 


* 


Y 


DIRECTION 








MATA( 2,2) 


= 1. 0D0 






MATA( 2,5) 


= Ml 






MATA( 2,11) 


= -1. 0D0 






MATB( 2) 


= 0. 0D0 


* 


Z 


DIRECTION 








MATA( 3,3) 


= 1.0D0 






MATA( 3,6) 


= Ml 






MATA( 3,12) 


= -1.0D0 






MATB( 3) 


= -WG1 


* 


EQUATIONS AT 


' JOINT ZERO 


* 


X 


DIRECTION 








MATA(4,4) 


= 1.0D0 






MATA(4, 8) 


= RBG1( 3) 






MATA(4, 9) 


= -RBG1(2) 






MATB(4) 


= AXO - MICO 


* 


Y 


DIRECTION 








MATA( 5,5) 


= 1.0D0 






MATA(5 , 7) 


= -RBG1(3) 






MATA(5 , 9) 


= RBG1(1) 






MATB(5) 


= AYO - MJCO 


* 


Z 


DIRECTION 








MATA( 6,6) 


= 1.0D0 






MATA(6, 7) 


= RBG1( 2) 






MATA( 6,8) 


= -RBGl(l) 



VcjWoWc 



+ TEMP 



TEMP 
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MATB( 6) = AZO - MKCO 



SUM OF MOMENTS EQUATIONS 



* 


X DIRECTION 






MATA( 7,2) = 


RBG1C3) 




MATA( 7,3) = ■ 


■RBG1C 2) 




MATAC 7,7) = ■ 


■IXXTC1) 




MATA( 7,8) = 


IXYTC1) 




MATA( 7,9) = 


IXZTC1) 




MATA( 7,11) = ■ 


■RAG1C 3) 




MATA( 7,12) = 


RAG1C 2) 




MATB ( 7 ) 


T1X - TOX 


•>v 


Y DIRECTION 






MATA( 8,1) = ■ 


•RBG1C 3) 




MATA( 8,3) = 


RBGl(l) 




MATAC 8,7) = 


IXYT( 1) 




MATA( 8, 8) = ■ 


■IYYT(l) 




MATAC 8,9) = 


IYZT(l) 




MATA( 8, 10) = 


RAG1C3) 




MATAC 8, 12) = ■ 


■RAGl(l) 




MATB C 8 ) 


T1Y - TOY 


* 


Z DIRECTION 






MATAC 9,1) = 


RBG1C 2) 




MATAC 9, 2) = ■ 


■RBG1C1) 




MATAC 9, 7) = 

MATAC 9, 8) = 


IXZTC1) 

IYZTC1) 




MATAC 9, 9) = • 


-IZZT(l) 




MATAC 9, 10) = ■ 


■RAG1C2) 




MATAC 9, 11) = 


RAGl(l) 




MATB( 9) 


T1Z - TOZ 


** 


LINK TWO 




Vc 


SUM OF FORCES 




* 


X DIRECTION 




460 


MATAC 10, 10) = 


1. ODO 




MATAC 10, 13) = 


M2 




MATAC 10, 19) = 


-1. ODO 




MATB CIO) 


0. ODO 


Vc 


Y DIRECTION 






MATAC 11, 11) = 


1. ODO 




MATAC 11, 14) = 


M2 




MATAC 11,20) = 


-1. ODO 




MATB C 1 1 ) 


0. ODO 


Vc 


Z DIRECTION 






MATAC 12, 12) = 


1. ODO 




MATAC 12, 15) = 


M2 




MATAC 12,21) = 


-1. ODO 




MATB C 12) 


-WG2 


* 


EQUATIONS AT JOINT ONE 


y- 


X DIRECTION 






MATAC 13,4) = 


-1. ODO 




MATAC 13,8) = 


-RAG1C3) 




MATAC 13,9) = 


RAG1C 2) 




MATAC 13,13) = 


1. ODO 




MATAC 13, 17) = 


RBG2C3) 




MATAC 13,18) = 


-RBG2C2) 




MATB(13) 


mici - m: 


ic 


Y DIRECTION 
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MATA( 14,5) 
MATA( 14,7) 
MATA( 14,9) 
MATA( 14 , 14) 
MATA( 14,16) 
MATA( 14,18) 
MATB( 14) 

Z DIRECTION 
MATA( 15,6) 
MATA( 15,7) 
MATA( 15,8) 
MATA( 15,15) 
MATA( 15,16) 
MATA( 15,17) 
MATB( 15) 

SUM OF MOMENTS 
X DIRECTION 
MATA( 16,11) 
MATA( 16,12) 
MATA( 16,16) 
MATA( 16,17) 
MATA( 16,18) 
MATA( 16,20) 
MATA( 16,21) 
MATB( 16) 

Y DIRECTION 
MATA( 17 , 10) 
MATA( 17,12) 
MATA( 17,16) 
MATA( 17,17) 
MATA( 17,18) 
MATA( 17,19) 
MATA( 17,21) 
MATB( 17) 

Z DIRECTION 
MATA( 18,10) 
MATA( 18,11) 
MATA( 18,16) 
MATA( 18,17) 
MATA( 18,18) 
MATA( 18,19) 
MATA( 18,20) 
MATB( 18) 

LINK THREE 
SUM OF FORCES 
X DIRECTION 
MATA( 19,19) 
MATA( 19,22) 
MATB( 19) 

Y DIRECTION 
MATA( 20,20) 
MATA( 20,23) 
MATB( 20) 

Z DIRECTION 
MATA( 21,21) 
MATA( 2 1 , 24) 



= -1. 0D0 
= RAG1( 3) 

= -RAGl(l) 

= 1. ODO 

= -RBG2( 3 ) 

= RBG2( 1) 

= MJC1 - MJC2 

= -1. ODO 
= -RAG1( 2 ) 

= RAG1(1) 

= 1. ODO 

= RBG2( 2) 

= -RBG2( 1) 

= MKC1 - MKC2 
EQUATIONS 

= RBG2( 3 ) 

= -RBG2( 2) 

= -IXXT( 2) 

= IXYT( 2) 

= IXZT( 2) 

= -RAG2( 3) 

= RAG2( 2) 

= T2X - T1X 

= -RBG2( 3) 

= RBG2( 1) 

= IXYT( 2) 

= -IYYT( 2) 

= IYZT( 2) 

= RAG2( 3) 

= -RAG2( 1) 

= T2Y - T1Y 

= RBG2( 2) 

= -RBG2( 1) 

= IXZT( 2) 

= IYZT( 2) 

= -IZZT( 2) 

= -RAG2( 2) 

= RAG2( 1) 

= T2Z - T1Z 



= 1. ODO 

= M3 
= 0. ODO 

= 1. ODO 

= M3 
= 0. ODO 

= 1. ODO 

= M3 



* 

* 



* 



* 



* 

* 



* 



* 



MATB (21) = -WG3 
EQUATIONS AT JOINT 
X DIRECTION 
MATA( 22,13) 

MATA( 22,17) 

MATA( 22,18) 

MATA( 22,22) 

MATA( 22,26) 

MATA( 22,27) 

MATB( 22) 

Y DIRECTION 
MATA( 23,14) 

MATA( 23,16) 

MATA( 23,18) 

MATA( 23,23) 

MATA( 23,25) 

MATA( 23,27) 

MATB( 23) 

Z DIRECTION 
MATA( 24,15) 
MATA(24, 16) 

MATA( 24,17 : 

MATA( 24,24) 

MATA( 24,25) 

MATA( 24,26) 

MATB (24) 

SUM OF MOMENTS 
X DIRECTION 
MATA( 25,20) 

MATA( 25,21) 

MATA( 25,25) 

MATA( 25,26) 

MATA( 25,27) 

MATB (25) 

Y DIRECTION 
MATA(26, 19) 

MATA( 26,21) 

MATA( 26,25) 

MATA( 26,26) 

MATA( 26,27) 

MATB (26) 

Z DIRECTION 
MATA(27 ,19) 

MATA( 27,20) 

MATA( 27,25) 

MATA( 27,26) 

MATA( 27,27) 

MATB (27) 



TWO 



= -1. ODO 
= -RAG2( 3) 

= RAG2( 2) 

= 1. ODO 

= RBG3( 3) 

= -RBG3( 2) 

= MIC 3 - MIC4 

= - 1. ODO 
= RAG2( 3) 

= -RAG2(1) 

= 1. ODO 

= -RBG3(3) 

= RBG3( 1) 

= MJC3 - MJC4 

= - 1. ODO 
= -RAG2(2) 

= RAG2( 1) 

= 1. ODO 

= RBG3( 2) 

= -RBG3( 1) 

= MKC3 - MKC4 
EQUATIONS 

= RBG3( 3) 

= -RBG3( 2) 

= -IXXT( 3) 

= IXYT( 3) 

= IXZT( 3) 

= -T2X 

= -RBG3( 3) 

= RBG3( 1) 

= IXYT( 3) 

= -IYYT( 3) 

= IYZT( 3) 

= -T2Y 

= RBG3( 2) 

= -RBG3( 1) 

= IXZT( 3) 

= IYZT( 3) 

= -IZZT( 3) 

= -T2Z 



***** FIRST LINK IS CONSTRAINED TO ROTATE IN Z DIRECTION 
*** MODIFIED BY RM VERBOS 
590 DO 600 I = 4,8 

DO 595 J = 1,27 
MATA( I , J) = 0. ODO 
* MATA( J, I) = 0. ODO 

595 CONTINUE 



***** 
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600 



605 



610 



* 

620 



624 



627 



630 

631 

633 

635 



VoWc 

* 

640 



* 

* 



*660 

* 

* 



MATB(I) = 0. 0D0 
CONTINUE 

MATA( 4,4) = 1.0D0 
MATA( 5,5) = 1.0D0 
MATA( 6,6) = 1.0D0 
MATA( 7,7) = 1. 0D0 
MATA( 8,8) = 1.0D0 



DO 610 J = 1,27 

MATA( 18, J) = 0. 0D0 
MATA( 27 , J) = 0. 0D0 
CONTINUE 



MATA( 9,9) 
MATA( 18,9) 
MATA( 18,18) 
MATB( 18) 
MATA( 27,9) 
MATA( 27,27) 
MATB( 27) 



-(IZZT(1)+IZZT(2)+IZZT(3)) 
-1. ODO 
1. ODO 

0. ODO 
-1. ODO 

1. ODO 
0. ODO 



CALL EQUATION SOLVER PROGRAM FROM IMSL 

CALL LEQT2F( MATA , M , N , I A , MATB , IDGT , WKAREA , IER) 

IF (IER. EQ. 0) THEN 

GOTO 640 

ELSE 

WRITE (*,624) IER 
FORMAT (15) 

DO 635 I = 1, 27 
WRITE (*,627) I 
FORMAT (17) 

DO 631 J = 1, 27, 3 

WRITE (*,630) J,MATA( I , J) , J+l ,MATA( I , J+l) , J+2 ,MATA( I , J+2) 
FORMAT ( 15 ,F13. 5 , 15 ,F13. 5,I5,F13. 5) 

CONTINUE 

WRITE (*,633) I , MATB ( I ) 

FORMAT (I3,F15.5) 

CONTINUE 
CALL ENDJOB 
END IF 



FIND LCOGX , LCOGY , LCOGZ , THETA VALUES ,WX,WY,WZ 

JOINT ZERO 

FXO = MATB(l) 

FYO = MATB( 2) 

FZO = MATB(3) 

LINK ONE 

SINCE LINK1 IS CONSTRAIN TO ROTATE AROUND THE Z ONLY 
AX1 = 0. ODO 
AY1 = 0. ODO 
AZ1 = 0. ODO 
AX1 = MATB( 4) 

VELX1 = INTGRL( 0. ,AX1) 

LCOGX 1 = INTGRL(X1 , VELX1) 

LCOGX(l) = LCOGX 1 
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* 


AY1 


= 


MATB( 5) 


* 


VELY1 




INTGRL( 0. , AY1) 


* 


LCOGY1 


= 


INTGRL( Y1 , VELY1) 


■sV 


LCOGY(l) 


= 


LCOGY1 


V? 


AZ1 


= 


MATB( 6) 


Vc 


VELZ1 


= 


INTGRL( 0. , AZ1) 


* 


LCOGZ1 


= 


INTGRL( Z 1 , VELZ 1 ) 


Vf 


LCOGZ(l) 


= 


LCOGZ1 



*** 

685 



VD1X 

W1X 

WDX(l) 

Wl(l) 

WD1Y 

W1Y 

WDY(l) 

Wl(2) 

WD1Z 

W1Z 

WDZ(l) 

Wl(3) 

ADDED BY R. 
THZ 

THZANG 
COSTHZ = 
SINTHZ 



• MATB( 7) 

= INTGRL(0. ,WD1X) 
= WD1X 
= W1X 
: MATB( 8) 

= INTGRL( 0. ,WD1Y) 
= WD1Y 
= W1Y 
= MATB(9) 

= INTGRL( 0. ,WD1Z) 
= WD1Z 
= W1Z 
M. VERBOS 
= INTGRL( 0. ,W1Z) 

= THZ * RADEG 
■ DCOS(THZ) 

= DSIN(THZ) 



* IF THE 1ST LINK IS CONSTRAIN TO ROTATE IN THE Z DIRECTION ONLY 

* THE DIRECTION COSINE AND DIRECTION COSINE ANGLES ARE CONSTANT 

* AND DO NOT NEED TO BE CALCULATED 

* CALC DIRECTIONAL COSINES FOR LINK ONE 

Vc 

*700 DRCSX(l) = LCOGX1 / LNCOG1 

* DRCSY(l) = LCOGY1 / LNCOG1 

* DRCSZ(l) = LCOGZ1 / LNCOG1 

* AMP = DSQRT(DRCSX( 1)*DRCSX( 1)+DRCSY( 1)*DRCSY( 1)+. . . 



DRCSZ( 1)*DRCSZ( 1) ) 





DRCSX(l) = DRCSX( 1)/AMP 






* 


DRCSY(l) = DRCSY( 1) /AMP 








DRCSZ(l) = DRCSZ( 1)/AMP 






*720 


DRCANX(l) = DACOS( DRCSX( 1 ) ) 


* 


RADEG 




DRCANY(l) = DACOS( DRCSY( 1) ) 


* 


RADEG 


* 


DRCANZ(l) = DACOS(DRCSZ(l)) 


* 


RADEG 


** 


JOINT LOCATION 






*730 


JX1 = LINKL1 * DRCSX(l) 








JY1 = LINKL1 * DRCSY(l) 






* 


JZ1 = LINKL1 * DRCSZ(l) 






JU 


JOINT ONE 






735 


FX1 = MATB(IO) 








FY1 = MATB(ll) 








FZ1 = MATB( 12) 








LINK TWO 






740 


AX2 = MATB( 13) 







VELX2 = INTGRL( 0. ,AX2) 
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LC0GX2 


= 


INTGRL( X2 , VELX2 ) 


LCOGX( 2) 


= 


LCOGX2 


AY2 




MATB( 14) 


VELY2 


= 


INTGRL( 0. , AY2) 


LC0GY2 


= 


INTGRL( Y2 , VELY2) 


LCOGY( 2) 


= 


LCOGY2 


AZ2 


= 


MATB( 15) 


VELZ2 


= 


INTGRL(0. , AZ2 ) 


LC0GZ2 


= 


INTGRL( Z2 , VELZ2) 


LCOGZ( 2 ) 


= 


LCOGZ2 


WD2X 


= 


MATB( 16) 


W2X 


= 


INTGRL(0. ,WD2X) 


WDX( 2) 


— 


WD2X 


W2( 1) 




W2X 


WD2Y 


= 


MATB( 17) 


W2Y 


zz 


INTGRL(0. ,WD2Y) 


WDY( 2) 


— 


WD2Y 


W2( 2) 


— 


W2Y 


WD2Z 


= 


MATB( 18) 


W2Z 


zz 


INTGRL( 0. , WD2Z) 


WDZ( 2) 


= 


WD2Z 


W2( 3) 


— 


W2Z 



* GET THE DIRECTION COSINES FOR THE LINK TWO 

DRCSX( 2) = (LCOGX2 - JX1) / LNC0G2 

DRCSY( 2) = ( LCOGY2 - JY1) / LNC0G2 

DRCSZ( 2) = ( LCOGZ2 - JZ1) / LNCOG2 

790 AMP = DSQRT(DRCSX(2)*DRCSX(2)+DRCSY(2)*DRCSY(2)+. . . 

DRCSZ( 2)*DRCSZ( 2) ) 

DRCSX(2) = DRCSX( 2) /AMP 

DRCSY( 2) = DRCSY( 2) /AMP 

DRCSZ( 2) = DRCSZ( 2) /AMP 

DRCANX( 2) = DACOS(DRCSX( 2) ) * RADEG 

DRCANY( 2) = DAC0S(DRCSY(2)) * RADEG 

DRCANZ( 2) = DACOS(DRCSZ( 2) ) * RADEG 

* JOINT LOCATION 

800 JX2 = JX1 + LINKL2 * DRCSX(2) 

JY2 = JY1 + LINKL2 * DRCSY(2) 

JZ2 = JZ1 + LINKL2 * DRCSZ(2) 

* JOINT TWO 

805 FX2 = MATB( 19) 

FY2 = MATB( 20) 

FZ2 = MATB(21) 



LINK THREE 




AX 3 


= MATB( 22) 


VELX3 


= INTGRL( 0. , AX3) 


LCOGX3 


= INTGRL(X3 , VELX3) 


LCOGX( 3) 


= LC0GX3 


AY3 


= MATB( 23) 


VELY3 


= INTGRL( 0. , AY3) 


LC0GY3 


= INTGRL( Y3 , VELY3) 


LCOGY( 3) 


LC0GY3 


AZ3 


= MATB( 24) 


VELZ3 


= INTGRL( 0. , AZ3) 
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LC0GZ3 


= 


INTGRL( Z3 , VELZ3 ) 


LCOGZ( 3) 




LC0GZ3 


WD3X 


= 


MATB( 25) 


W3X 


= 


INTGRL( 0. ,WD3X) 


WDX( 3) 


= 


WD3X 


W3( 1) 


= 


W3X 


WD3Y 


= 


MATB( 26) 


W3Y 


= 


INTGRL( 0. ,WD3Y) 


WDY ( 3 ) 




WD3Y 


W3( 2) 


= 


W3Y 


WD3Z 


= 


M ATB (27) 


W3Z 


= 


INTGRL(0. ,WD3Z) 


WDZ( 3) 


— 


WD3Z 


W3( 3) 


= 


W3Z 



* CALC DIRECTIONAL COSINES FOR LINK THREE 
845 DRCSX( 3 ) = (LCOGX3 - JX2) / LNCOG3 

DRCSY( 3 ) = (LC0GY3 - JY2) / LNCOG3 
DRCSZ( 3) = (LC0GZ3 - JZ2) / LNCOG3 
865 AMP = DSQRT(DRCSX(3)*DRCSX(3)+DRCSY(3)*DRCSY(3)+. . . 

DRCSZ( 3)*DRCSZ( 3) ) 

DRCSX( 3) = DRCSX( 3)/AMP 

DRCSY( 3) = DRCSY(3)/AMP 

DRCSZ( 3) = DRCSZ( 3)/AMP 

DRCANX(3) = DACOS( DRCSX( 3 ) ) * RADEG 

DRCANY( 3) = DACOS(DRCSY( 3) ) * RADEG 

DRCANZ( 3) = DACOS(DRCSZ( 3) ) * RADEG 



* TIP LOCATION 

875 TIPX = JX2 + LINKL3 * DRCSX(3) 

TIPY = JY2 + LINKL3 * DRCSY(3) 
TIPZ = JZ2 + LINKL3 * DRCSZ(3) 



* 

880 



FIND THE 
ANG23X = 
ANG23Y = 
ANG23Z = 



ANGLE BETWEEN THE LIMKS 
DRCANX( 2) - DRCANX( 3 ) 
DRCANY( 2) - DRCANY( 3) 
DRCANZ( 2) - DRCANZ( 3) 



ANG12X = DRCANX(l) 
ANG12Y = DRCANY(l) 
ANG12Z = DRCANZ(l) 



DRCANX( 2) 
DRCANY( 2) 
DRCANZ( 2) 



DYNAMIC 

NOSORT 

***** input TORQUE AT JOINTS 

* CALCULATE THE MAGNITUDE OF THE LENGTH OF THE PROJECTION OF 

* LINK 2 AND 3 IN THE X Y PLANE WHICH IS THE MOMENT ARM FOR 

* THE CALCULATION OF THE GRAVITATIONAL TORQUE 

900 MARM2A = DSQRT ( LC0GX2 * LC0GX2 + LC0GY2 * LC0GY2 ) 

MARM2B = DSQRT ( LC0GX3 * LC0GX3 + LC0GY3 * LC0GY3 ) 
MARM3 = DSQRT ( (LC0GX3 - JX2) * ( LC0GX3 - JX2) +. . . 

(LC0GY3 - JY2) * ( LC0GY3 - JY2) ) 

* CALCULATE GRAVITATIONAL TORQUES 
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TGI = MARM2A * WG2 + MARM2B * WG3 
TG2 = MARM3 * WG3 



Vc 

*10 

* 

* 

* 

* 



INPUT TORQUES OVERTOP OF GRAVITY 

T1FNC = 2* A - A * DCOS ( W * TIME ) 

T2FNC = -3 * A * DSIN (W*TIME) 

TOZ = A * DSIN ( W * TIME ) 

TOZ = A - A * DSIN ( W * TIME ) 

TOZ = 50. 0 - 50. 0 * TIME 



* CALCULATE TOTAL TORQUE ON JOINT 1 AND 2 
T1 = TGI + T1FNC 
T2 = TG2 + T2FNC 



* CALCULATE X AND Y COMPONENT OF TORQUE ON JOINT 1 AND 2 

T1X = T1 * DCOS (THZ) 

T1Y = T1 * DSIN (THZ) 

T2X = T2 * COS (THZ) 

T2Y = T2 * SIN (THZ) 

* TORQUE CHECK 

T1CH = DSQRT ( T1X * T1X + T1Y * T1Y ) 

T2CH = DSQRT ( T2X * T2X + T2Y * T2Y ) 

END 

STOP 

FORTRAN 

* SUBROUTINE TO COMPUTE THE CROSS PRODUCT OF TWO VECTORS 

SUBROUTINE CPROD( VECTA , VECTB , MI , MJ , MK) 

940 IMPLICIT REAL*8 (A-Z) 

DIMENSION VECTA( 3) , VECTB( 3) 

MI = VECTA( 2) * VECTB( 3) - VECTA(3) * VECTB(2) 
MJ = VECTA( 3) * VECTB ( 1) - VECTA(l) * VECTB(3) 
MK = VECTA(l) * VECTB( 2) - VECTA(2) * VECTB(l) 
RETURN 

END 
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APPENDIX B 



DOUBLE PENDULUM VALIDATION PROGRAM 



TITLE DOUBLE PENDULUM VALIDATION PROGRAM 



************************************************************************* 



* THIS IS A PROGRAM THAT WILL SOLVE THE DIRECT DYNAMICS PROBLEM FOR A * 

* 2 LINK RIGID BODY MANIPULATOR WITHOUT THE USE OF THE STANDARD TRAN- * 

* FORMATION MATRICES AND VALIDATE THE MOTION BY THE USE OF THE DOUBLE * 

* PENDULUM PROBLEM. * 



* LT R. M. VERBOS USN 



TERMINAL 
METHOD ADAMS 

PRINT . 10 , ANGTH2 , TH2ANG , ANGTH3 ,TH3ANG , ANG23 

CONTROL FINTIM =4.0, DELT = .0005, DELMAX = .1, DELPRT = .10 

SAVE .05, ANGTH2 , TH2ANG , ANGTH3 , TH3ANG , ANG23 

GRAPH (DE=TEK618) TIME , ANGTH2 , TH2ANG 

GRAPH ( DE=TEK618) TIME , ANGTH3 , TH3ANG 

GRAPH ( DE=TEK618) TIME , ANG23 

D DIMENSION MATA( 27 , 27) ,MASS( 3 , 2) , L( 3 , 2) ,RX(3,2) ,RY(3,2) ,RZ(3,2) 

D DIMENSION IXX( 3 ,2) , IXZ( 3,2) , IXY( 3 ,2) , IYY(3 ,2) , IYZ( 3, 2) , IZZ( 3 ,2) 

D DIMENSION I MAT( 3,3,3), NIMAT( 3,3), TMAT( 3,3), TMATTR( 3,3), MATTMP( 3,3) 

D DIMENSION LIMAT(3,3,3) 

FIXED IER, I , J,M,K,P,N, IA, IDGT,A, II 

ARRAY MATB( 27) ,ABMATB(27) ,LCOGX(3) ,LCOGY( 3) ,LCOGZ(3) 

ARRAY VECTA0( 3) , VECTB0( 3) , VECTA1( 3) ,VECTB1(3) ,VECTA2(3) ,VECTB2(3) 

ARRAY VECTA( 3) , VECTB( 3) 

ARRAY WDX( 3) ,WDY(3) , WDZ( 3) ,W1( 3) ,W2(3) ,W3(3) 

ARRAY RBG1( 3) ,RAG1(3) ,RBG2( 3) ,RAG2( 3) ,RBG3(3) 

ARRAY WKAREA( 850) 

ARRAY IXXT( 3) , IYYT( 3) , IZZT( 3) , IXYT( 3) , IXZT( 3) , IYZT( 3) 

ARRAY DRC ANX( 3 ) , DRCANY( 3 ) , DRCANZ ( 3 ) 

ARRAY DRCSX( 3) ,DRCSY( 3) ,DRCSZ( 3) 

INITIAL 



IrkMrie 


INPUT ***** 


* 


INPUT PARAMETER CONSTANTS 


55 


A = 15. 0D0 
P = 0. 0D0 
W' = 2 * PI 

IDGT = 6 


*** 


MODIFIED BY RM VERBOS 
G = 9. 8 IDO 


* 


G = 0. 0D0 
N = 27 
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M = 1 
IA = 27 
IER = 0 
LEVELQ = 0 
LEVLDQ = 0 

INPUT JOINT LOCATIONS IN METERS 
JXO = O.ODO 
JYO = O.ODO 
JZO = 0. ODO 
JX1 = 0. ODO 
JY1 = O.ODO 
JZ1 = 0. 2D0 
JX2 = 0. ODO 
JY2 = 0.416D0 
JZ2 = 0.2D0 

INPUT TORQUE CONSTANTS 



TOX 


= 


0. ODO 


TOY 


= 


0. ODO 


TOZ 


= 


0. ODO 


T1X 


= 


0. ODO 


T1Y 


= 


0. ODO 


T1Z 


= 


0. ODO 


T2X 


= 


0. ODO 


T2Y 




0. ODO 


T2Z 


= 


0. ODO 



INPUT DISTANCE FROM CENTER OF LINK TO CENTER OF MASS 
FOR EACH LINK ENDS 
L(l,l) = 0. 05D0 
L( 1 , 2) = 0. 15D0 
L(2,l) = 0.20D0 
L( 2 , 2) = 0. 216D0 
L(3,l) = 0. 225D0 
L(3,2) = 0. 226D0 



* INPUT THE LINK 
LINKL1 
LINKL2 
LINKL3 



LENGTHS OF THE ROBOT 
= 0. 20D0 
= 0.416D0 
= 0.45 IDO 



* INPUT MASS AT LINK ENDS IN KILOGRAMS 

110 MASS( 1,1) = 11. ODO 

MASS( 1,2) = 33. ODO 
MASS( 2,1) = 2. 2D0 
MASS( 2,2) = 2.2D0 
MASS( 3,1) = 28. 6D0 
MASS( 3 ,2) = 28. 6D0 



* 

120 



INPUT LOCATION 
LCOGX(l) = 
XI 

LCOGY(l) = 
Y1 

LCOGZ(l) = 



OF LINK CENTERS OF GRAVITY 
0. ODO 
LCOGX(l) 

0. ODO 
LCOGY(l) 

0. 10D0 
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Z1 


= 


LCOGZ(l) 


LCOGX( 2 ) 


= 


0. ODO 


X2 


= 


LCOGX( 2) 


LCOGY( 2 ) 


= 


0. 208D0 


Y2 


= 


LCOGY( 2) 


LC0GZ(2) 


— 


0. 20D0 


Z2 


= 


LCOGZ( 2) 


LCOGX( 3) 


= 


0. ODO 


X3 


= 


LCOGX( 3) 


LCOGY( 3) 


n 


0. 6415D0 


Y3 


= 


LCOGY( 3) 


LCOGZ( 3) 




0. 2D0 


Z3 


= 


LC0GZ(3) 



* INPUT MASS OF EACH LINK IN KG 

Ml = 44. ODO 
M2 = 4.4D0 
M3 = 57. 2D0 

* INPUT ACCELERATIONS OF JOINT ZERO 

AXO = 0. ODO 
AYO = 0. ODO 
AZO = 0. ODO 



INPUT THE INITIAL DIRECTION COSINES 
DRCSX(l) = 0. ODO 
DRCSY(l) = 0. ODO 
DRCSZ(l) = 1. ODO 
DRCSX( 2) = 0. ODO 
DRCSY( 2) = 1. ODO 
DRCSZ( 2) = 0. ODO 
DRCSX( 3) = 0. ODO 
DRCSY( 3) = 1. ODO 
DRCSZ( 3) = 0. ODO 



Vr 



INPUT THE INITIAL DIRECTION COSINE ANGLES 



DRCANX(l) 
DRCANY(l) 
DRCANZ( 1) 
DRCANX( 2) 
DRCANY( 2) 
DRCANZ( 2) 
DRCANX( 3) 
DRCANY(3) 
DRCANZ( 3) 



90. ODO 
90. ODO 
0. ODO 
90. ODO 
0. ODO 
90. ODO 
90. ODO 
0. ODO 
90. ODO 



***** INITIALIZE ***** 

* OMEGA AND OMEGA DOT 

160 DO 170 I = 1,3 

W1(I) = 0. ODO 

W2( I ) = 0. ODO 

W3( I ) = 0. ODO 

WDX(I) = 0. ODO 

WDY(I) = 0. ODO 

WDZ(I) =0. ODO 

170 CONTINUE 
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THZ = 0. 0D0 



* INITIALIZE MATRIX A AND B TO ZERO 
DO 180 I = 1,27 
DO 175 J = 1,27 

MATA( I , J) = 0. 0D0 
175 CONTINUE 

MATB(I) = 0. ODO 
180 CONTINUE 



***** CALCULATIONS ***** 

* WEIGHTS (NEWTONS) 

185 WG1 = M1*G 

WG2 = M2*G 
WG3 = M3*G 

* COMPUTE THE LENGTH FROM THE INBOARD JOINT TO COG 

LNC0G1 = DSQRT ( LCOGX( 1)*LC0GX( 1) + LCOGY( 1 )*LCOGY( 1) +. . . 

LC0GZ(1)*LC0GZ(1) ) 

LX 2 = LCOGX( 2) - JX1 
LY2 = LCOGY( 2) - JY1 
LZ2 = LCOGZ( 2) - JZ1 

LNC0G2 = DSQRT ( LX2*LX2 + LY2*LY2 + LZ2*LZ2 ) 

LX 3 = LCOGX( 3) - JX2 
LY3 = LCOGY( 3) - JY2 
LZ3 = LCOGZ( 3) - JZ2 

LNC0G3 = DSQRT ( LX3*LX3 + LY3*LY3 + LZ3*LZ3 ) 



* 



190 



200 



205 



COMPUTE 

DO 



INITIAL INERTIAS BASED ON POINT MASSES 
IN GLOBAL COORDINATES 



* 

* 



225 I = 1,3 
RX( 1,1) = -L( 1,1) 

RX( 1,2) = L( I , 2) 

RY(I,i) = -L(I,1) 

RY ( I , 2 ) = L( I , 2) 

RZ( 1,1) = -L( 1,1) 

RZ( 1,2) = L( I , 2) 
IXX(I,1) = MASS( 1,1) * ( (RY( 1,1) 
IXX( 1,2) = MASS( 1,2) * ( ( RY (1,2) 
IXXT(I) = IXX(I,1) + IXX( 1,2) 
IYY( 1,1) = MASS( 1,1) * ( (RX( 1,1) 
IYY( 1,2) = MASS( 1,2) * ((RX(I,2) 
IYYT(I) = IYY( I , 1) + IYY( 1,2) 
IZZ(I,1) = MASS( 1,1) * ( (RX( 1,1) 
IZZ( 1,2) = MASS( 1,2) * ( (RX( 1,2) 
IZZT(I) = IZZ( 1,1) + IZZ( 1,2) 
IXY( 1,1) = MASS ( I , 1) * RX( 1,1) * 
IXY( 1,2) = MASS( 1,2) * RX( 1,2) * 
IXYT(I) = IXY( 1,1) + I XY ( I , 2 ) 
IXZ(I,1) = MASS( 1,1) * RZ( 1,1) * 
IXZ( 1,2) = MASS( 1,2) * RZ( 1,2) 
IXZT(I) = IXZ(I,1.) + 

IYZ( 1,1) = MASS( 1,1) 

IYZ( 1,2) = MASS( 1,2) 

IYZT(I) = IYZ( 1,1) + 



DRCSX(I) 

DRCSX(I) 

DRCSY(I) 

DRCSY(I) 

DRCSZ(I) 

DRCSZ(I) 

RY(I,1)) 
RY(I,2)) 



* 



* 



* 



RX(I,l)) 

RX(I,2)) 

RX(I,l)) 
RX( 1,2)) 



IXZ( 1,2) 
? RY(I,1) 
• RY ( I , 2 ) 
IYZ( I , 2) 



RY( 1,1) 
RY( I ,2) 

RX( I , 1) 
RX( 1,2) 

RZ(I,1) 

RZ(I,2) 



(RZ( I ,1) 
(RZ( 1,2) 

(RZ( 1,1) 
(RZ( 1,2) 

(RY(I,1) 
( RY ( I , 2 ) 



* RZ( 1,1))) 

* RZ(I,2))) 

* RZ( I , 1) ) ) 

* RZ( 1,2))) 

* RY( I , 1) ) ) 

* RY( I , 2) ) ) 
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IF (IXXT(I) . LE. .01) THEN 

IXXT(I) = . 01 

ELSE 

IXXT(I) = IXXT(I) 

END IF 

IF (IYYT(I) .LE. .02) THEN 

IYYT(I) = .02 

ELSE 

IYYT(I) = IYYT(I) 

END IF 

IF (IZZT(I) .LE. .02) THEN 

IZZT(I) = . 02 

ELSE 

IZZT(I) = IZZT(I) 

END IF 

IMAT( 1,1,1) = IXXT(I) 
IMAT( 1,1,2) = IXYT(I) 
IMAT( 1,1,3) = IXZT(I) 
IMAT( 1,2,1) = -IXYT(I) 
IMAT( 1,2,2) = IYYT(I) 
IHAT( 1,2,3) = IYZT(I) 
IMAT( 1,3,1) = -IXZT(I) 
IHAT( 1,3,2) = -IYZT(I) 
IMAT( 1,3,3) = IZZT(I) 
225 CONTINUE 



* DUE TO LINK 1 CONSTRAINTS LINK 1 INERTIAS ARE CONSTANT 

IXXT(l) = IMAT( 1,1,1) 

IXYT(l) = IMAT( 1,1,2) 

IXZT(l) = IMAT( 1,1,3) 

IYYT(l) = IMAT( 1,2,2) 

IYZT(l) = IMAT( 1,2,3) 

IZZT(l) = IMAT( 1,3,3) 



* 



TRANSFORM THE INITIAL 
DO 9 I = 2, 3 
TMAT( 2,1) 

TMAT( 2,2) 

TMAT( 2,3) 

TMAT( 3,1) 

TMAT( 3,2) 

TMAT( 3,3) 



INERTIAS TO LOCAL COORDINATED 



-DCOS 
-DSIN 
0. ODO 
DRCSX(I) 
DRCSY(I) 
DRCSZ(I) 



THZ 

THZ 



VECTA(l) = TMAT( 2,1) 

VECTA( 2) = TMAT( 2,2) 

VECTA( 3) = TMAT( 2,3) 

VECTB(l) = TMAT( 3,1) 

VECTB( 2) = TMAT( 3,2) 

VECTB( 3) = TMAT( 3,3) 

CALL CPROD ( VECTA , VECTB , M I 1 , M J 1 , MK 1 ) 

TMAT( 1,1) = Mil 

TMAT( 1,2) = MJ1 

TMAT( 1,3) = MK1 

TMATTR( 1,1) = TMAT( 1,1) 

TMATTR( 1,2) = TMAT(2,1) 

TMATTR( 1,3) = TMAT(3,1) 
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TMATTR (2,1) = TMAT(1,2) 

TMATTR( 2,2) = TMAT(2,2) 

TMATTR ( 2,3) = TMAT(3,2) 

TMATTR ( 3,1) = TMAT(1,3) 

TMATTR ( 3,2) = TMAT(2,3) 

TMATTR (3, 3) = TMAT(3,3) 

DO 5 II = 1,3 
DO 5 J =1,3 
TEMP = 0. 0D0 
DO 1 K =1,3 

TEMP = THAT ( I 1 , K ) * IMAT(I,K,J) 
1 CONTINUE 

MATTMP(I1, J) = TEMP 
5 CONTINUE 

DO 8 II = 1,3 

DO 8 J =1,3 

TEMP = O.ODO 
DO 7 K = 1,3 

TEMP = MATTMP( II ,K) * TMATTR(K, 

7 CONTINUE 

LIMAT( I , I 1 , J) = TEMP 

8 CONTINUE 

9 CONTINUE 



* DOUBLE PENDULUM INITIALIZATION 
TH2D0 = 0. ODO 
TH3D0 = O.ODO 
TH30 = PI/2.0 
TH20 = PI/2.0 
L2 = 0. 416D0 

L3 = 0.45 IDO 

M2P =30. 8D0 
M3P = 28. 6D0 
HF = 0. 5D0 

DERIVATIVE 

NOSORT 



230 


CALL ERRSET (208,256,-1,1,1) 
CALL UERSET( LEVELQ , LEVLDQ) 


* 


INITIALIZE MATRIX A AND B TO ZERO 
DO 240 I = 1,27 
DO 235 J = 1,27 

MATA( I , J) = O.ODO 


235 


CONTINUE 
MATB(I) = 0. ODO 


240 


CONTINUE 


* 


INPUT JOINT EQUATIONS 


* 


JOINT ZERO EQUATIONS 


0% 


W1 X (W1 X RB/G1) 


•J* 


RBGl(l) = JXO - LCOGX(l) 



+ TEMP 



+ TEMP 
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RBG 1 ( 2) = JYO - LCOGY(l) 

RBG1( 3 ) = JZO - LCOGZ(l) 

VECTA(l) = Wl(l) 

VECTA( 2) = Wl( 2) 

VECTA( 3) = Wl( 3) 

CALL CPROD( VECTAO ,RBG1 , MICO , MJCO , MKCO) 
VECTBO(l) = MICO 
VECTBO( 2) = MJCO 
VECTBO( 3) = MKCO 

CALL CPROD( VECTAO, VECTBO, MICO, MJCO, MKCO) 
SINCE LINK ONE DOES NOT MOVE 

MICO = 0. ODO 

MJCO = 0. ODO 

MKCO = 0. ODO 

INPUT JOINT EQUATIONS 
JOINT ZERO EQUATIONS 
W1 X (W1 X RB/G1) 

RBG1(1) = JXO - LCOGX(l) 

RBG1( 2) = JYO - LCOGY(l) 

RBG1(3) = JZO - LCOGZ(l) 

VECTA(l) = Wl(l) 

VECTA( 2) = Wl( 2) 

VECTA( 3) = Wl( 3) 

CALL CPROD( VECTA , RBG1 , MICO , MJCO , MKCO ) 
VECTB(l) = MICO 
VECTB( 2) = MJCO 
VECTB( 3) = MKCO 

CALL CPROD( VECTA , VECTB , MICO , MJCO , MKCO ) 
W1 X (W1 X RA/G1) 

RAG1(1) = JX1 - LCOGX(l) 

RAG1( 2) = JY1 - LCOGY(l) 

RAG1( 3) = JZ1 - LCOGZ(l) 

VECTA(l) = Wl(l) 

VECTA( 2) = Wl( 2) 

VECTA( 3) = Wl(3) 

CALL CPROD (VECTA, RAG1 ,MIC1 ,MJC1 ,MKC1) 
VECTB(l) = MIC1 
VECTB ( 2) = MJC1 
VECTB( 3) = MKC1 

CALL CPROD (VECTA, VECTB, MIC1 ,MJC1 ,MKC1) 
W2 X ( V2 X RB/G2) 

RBG2( 1) = JX1 - LCOGX( 2) 

RBG2( 2) = JY1 - LCOGY( 2) 

RBG2( 3) = JZ1 - LCOGZ( 2) 

VECTA(l) = W2( 1) 

VECTA( 2) = W2( 2) 

VECTA( 3) = W2( 3) 

CALL CPROD (VECTA, RBG2 ,MIC2 ,MJC2 ,MKC2) 
VECTB ( 1) = MIC2 
VECTB( 2) = MJC2 
VECTB( 3) = MKC2 

CALL CPROD (VECTA, VECTB, MIC2 ,MJC2 ,MKC2) 
W2 X (W2 X RA/G2) 

RAG2( 1) = JX2 - LCOGX( 2) 

RAG2 ( 2 ) = JY2 - LC0GY(2) 



RAG2( 3) = JZ2 - LCOGZ( 2) 

VECTA(l) = W2( 1) 

VECTA( 2) = W2( 2) 

VECTA( 3) = W2( 3 ) 

CALL CPROD ( VECTA ,RAG2 , MIC3 ,MJC3 ,MKC3) 
VECTB(l) = MIC3 
VECTB( 2) = MJC3 
VECTB( 3) = HKC3 

CALL CPROD( VECTA , VECTB , MIC3 , MJC3 , MKC3 ) 
W3 X (W3 X RB/G3) 

RBG3( 1) = JX2 - LCOGX( 3) 

RBG3( 2) = JY2 - LCOGY(3) 

RBG3( 3) = JZ2 - LCOGZ( 3) 

VECTA(l) = W3( 1) 

VECTA( 2) = W3( 2) 

VECTA( 3) = W3( 3) 

CALL CPROD ( VECTA, RBG3,MIC4,MJC4,MKC4) 
VECTB(l) = MIC4 
VECTB( 2) = MJC4 
VECTB( 3) = MKC4 

CALL CPROD (VECTA, VECTB, MIC4,MJC4,MKC4) 

INERTIA TRANSFORMATION 
DO 390 I = 2, 3 

TMAT( 2,1) = -DCOS ( THZ ) 

TMAT( 2,2) = -DSIN ( THZ ) 

TMAT( 2,3) = 0. 0D0 
TMAT( 3,1) = DRCSX(I) 

TMAT( 3,2) = DRCSY(I) 

TMAT( 3,3) = DRCSZ(I) 

VECTA(l) = TMAT( 2,1) 

VECTA( 2) = TMAT( 2,2) 

VECTA( 3) = TMAT( 2,3) 

VECTB ( 1) = THAT (3,1) 

VECTB( 2) = TMAT( 3,2) 

VECTB( 3) = TMAT( 3,3) 

CALL CPROD (VECTA, VECTB, MI 1,MJ1,MK1) 

TMAT( 1,1) = Mil 

TMAT( 1,2) = MJ1 

TMAT( 1,3) = MK1 

TMATTR( 1,1) = TMAT( 1,1) 

TMATTR( 1,2) = TMAT(2,1) 

TMATTR( 1,3) = TMAT(3,1) 

TMATTR( 2,1) = TMAT(1,2) 

TMATTR(2,2) = TMAT(2,2) 

TMATTR( 2,3) = TMAT(3,2) 

TMATTR( 3,1) = TMAT(1,3) 

TMATTR( 3,2) = TMAT(2,3) 

• TMATTR( 3,3) = TMAT(3,3) 

DO 375 II = 1,3 
DO 375 J = 1,3 
TEMP = 0. ODO 
DO 370 K = 1,3 
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TEMP = TMATTR( I 1 ,K) * LIMAT( I ,K 
370 CONTINUE 

MATTMP( 1 1 , J) = TEMP 
375 CONTINUE 



378 

380 



DO 380 II = 1,3 
DO 380 J = 1,3 
TEMP = 0. 0D0 
DO 378 K = 1,3 

TEMP = MATTMP( 1 1 ,K) * TMAT(K, J) 
CONTINUE 

NIMAT( I 1 , J) = TEMP 
CONTINUE 



IXXT(I) 

IXYT(I) 

IXZT(I) 

IYYT(I) 

IYZT(I) 

IZZT(I) 



NIMAT( 1,1) 
NIMAT( 1,2) 
NIMAT( 1,3) 
NIMAT( 2,2) 
NIMAT( 2,3) 
NIMAT(3,3) 



390 



CONTINUE 



***** ENTER CONSTANTS INTO MATRIX A ***** 



■sV 


SUM OF FORCES 




Vc 


X 


DIRECTION 




395 




MATA( 1,1) = 


1. ODO 






MATA( 1,4) = 


Ml 






MATA( 1,10) = 


-1. ODO 






MATB(l) 


0. ODO 


Vc 


Y 


DIRECTION 








MATA( 2,2) = 


1. ODO 






MATA( 2,5) = 


Ml 






MATA( 2,11) = 


-1. ODO 






MATB ( 2 ) 


0. ODO 


* 


Z 


DIRECTION 








MATA(3,3) = 


1. ODO 






MATA( 3,6) = 


Ml 






MATA( 3,12) = 


-1. ODO 






MATB(3) 


-WG1 


* 


EQUATIONS AT JOINT ZERO 


* 


X 


DIRECTION 








MATA(4 ,4) = 


1. ODO 






MATA(4 , 8) = 


RBG1( 3) 






MATA(4,9) = ■ 


-RBG1(2) 






MATB( 4) 


AXO - MICO 


* 


Y 


DIRECTION 








MATA(5 ,5) = 


1. ODO 






MATA(5 , 7) = ■ 


-RBG1(3) 






MATA(5 ,9) = 


RBG1(1) 






MATB(5) 


AYO - MJCO 


* 


Z 


DIRECTION 








MATA( 6,6) = 


1. ODO 






MATA( 6,7) = 


RBG1( 2) 



J) + TEMP 



+ TEMP 
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MATA( 6,8) = 




■RBG1( 1) 




MATB( 6) 




AZO - MKCO 


* 


SUM OF MOMENTS 


EQUATIONS 


* 


X DIRECTION 








MATA( 7,2) 


= 


RBG1(3) 




MATA( 7,3) 




-RBG1(2) 




MATA( 7,7) 


= 


-IXXT(l) 




MATA( 7,8) 


n 


IXYT( 1) 




MATA( 7,9) 


= 


IXZT( 1) 




MATA( 7,11) 


= 


-RAG1(3) 




MATA( 7,12) 


= 


RAG1( 2) 




MATB( 7) 


= 


T1X - TOX 


* 


Y DIRECTION 








MATA( 8,1) 


= 


-RBG1(3) 




MATA( 8,3) 




RBG1( 1) 




MATA( 8,7) 




IXYT(l) 




MATA( 8,8) 


* = 


-IYYT(l) 




MATA( 8,9) 




IYZT(l) 




MATA( 8,10) 


=r 


RAG1( 3) 




MATA(8, 12) 




-RAGl(l) 




MATB( 8) 


= 


T1Y - TOY 




Z DIRECTION 








MATA (9,1) 


= 


RBG1( 2) 




MAT A (9, 2) 




-RBGl(l) 




MATA( 9,7) 


= 


IXZT(l) 




MATA( 9,8) 


— 


IYZT( 1) 




MATA( 9,9) 




-IZZT( 1) 




MATA( 9,10) 


= 


-RAG1( 2) 




MATA(9 ,11) 


= 


RAG1( 1) 




MATB( 9) 




T1Z - TOZ 


** 


LINK TOO 






Vc 


SUM OF FORCES 




* 


X DIRECTION 







460 



* 



* 

j. 



MATA( 10,10) 
MATA( 10,13) 
MATA( 10,19) 
MATB( 10) 

Y DIRECTION 
MATA( 11,11) 
MATA( 11,14) 
MATA( 11,20) 
MATB(ll) 

Z DIRECTION 
MATA( 12,12) 
MATA( 12,15) 
MATA( 12,21) 
MATB( 12) 
EQUATIONS AT 
X DIRECTION 



= 1. 0D0 

= M2 
= -1. ODO 
= 0. ODO 

= 1. ODO 

= M2 
= -1. ODO 
= 0. ODO 

= 1. ODO 

= M2 
= -1. ODO 
= -WG2 
JOINT ONE 



MATA( 13,4) 


= 


-1. ODO 


MATA( 13,8) 


= 


-RAG1(3) 


MATA( 13,9) 


= 


RAG1( 2) 


MATA( 13,13) 




1. ODO 


MATA( 13,17) 


= 


RBG2( 3) 


MATA( 13,18) 


= 


-RBG2(2) 
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MIC1 - MIC2 



MATB( 13) 

Y DIRECTION 

MATA( 14,5) = -1. ODO 

MATA( 14,7) = RAG1( 3) 

MATA(14,9) = -RAGl(l) 

MATAf 14,14) = 1. ODO 

MATA( 14,16) = -RBG2(3) 
MATA( 14,18) = RBG2( 1) 
MATB( 14) = MJC1 - MJC2 

Z DIRECTION 

MATA( 15,6) = -1. ODO 

MATA( 15,7) = -RAG1(2) 

MATA( 15,8) = RAG1(1) 

MATA( 15,15) = 1. ODO 

MATA( 15,16) = RBG2( 2) 
MATA( 15,17) = -RBG2( 1) 
MATB( 15 ) = MKC1 - MKC2 

SUM OF MOMENTS EQUATIONS 



X DIRECTION 
MATA( 16,11) 
MATA( 16,12) 
MATA( 16,16) 
MATA( 16,17) 
MATA( 16,18) 
MATA( 16,20) 
MATA( 16,21) 
MATB( 16) 

Y DIRECTION 
MATA( 17,10) 
MATAf 17,12) 
MATA( 17,16) 
MATA( 17,17) 
MATA( 17,18) 
MATA( 17,19) 
MATA( 17,21) 
MATB( 17) 

Z DIRECTION 
MATA( 18,10) 
MATA( 18,11) 
MATA( 18,16) 
MATA( 18,17) 
MATA( 18,18) 
MATA( 18,19) 
MATA( 18,20) 
MATB( 18) 

LINK THREE 

SUM OF FORCES 

X DIRECTION 
MATA( 19,19) 
MATA( 19,22) 
MATB( 19) 

Y DIRECTION 
MATA( 20,20) 
MATA( 20,23) 



= RBG2( 3) 

= -RBG2( 2 ) 

= -IXXT( 2) 

= IXYT( 2) 

= IXZT( 2) 

= -RAG2( 3) 

= RAG2( 2) 

= T2X - T1X 

= -RBG2( 3) 

= RBG2( 1) 

= IXYT( 2) 

= -IYYT( 2 ) 

= IYZT( 2) 

= RAG2( 3) 

= -RAG2( 1) 

= T2Y - T1Y 

= RBG2( 2) 

= -RBG2( 1) 

= IXZT( 2) 

= IYZT( 2) 

= - IZZT( 2 ) 

= -RAG2( 2) 

= RAG2( 1) 

= T2Z - T1Z 



= 1. ODO 

= M3 
= 0. ODO 

= 1. ODO 

= M3 



0. 0D0 



* 



* 



* 



* 



* 



MATB( 20) 

Z DIRECTION 

MATA( 21,21) = 1. ODO 

MATA( 21,24) = M3 
MATB (21) = -WG3 
EQUATIONS AT JOINT TWO 
X DIRECTION 
MATA( 22,13) 

MATA( 22,17) 

MATA( 22,18) 

MATA( 22,22) 

MATA( 22,26) 

MATA( 22,27) 

MATB ( 22) 

Y DIRECTION 
MATA( 23,14) 

MATA( 23,16) 

MATA(23, 18) 

MATA( 23,23) 

MATA( 23,25) 

MATA( 23,27) 

MATB( 23) 

Z DIRECTION 
MATA( 24,15) 

MATA( 24,16) 

MATA( 24,17) 

MATA( 24,24) 

MATA( 24,25) 

MATA( 24,26) 

MATB (24) 

SUM OF MOMENTS 
X DIRECTION 
MATA( 25,20) 

MATA( 25,21) 

MATA( 25,25) 

MATA( 25,26) 

MATA( 25,27) 

MATB( 25) 

Y DIRECTION 
MATA( 26,19) 
MATA(26,21) 

MATA( 26,25) 

MATA( 26 , 26) 

MATA( 26,27) 

MATB (26) 

Z DIRECTION 
MATA( 27,19) 

MATA( 27,20) 

MATA( 27,25) 

MATA( 27 , 26) 

MATA( 27,27) 

MATB( 27) 



-1. ODO 
-RAG2( 3) 

= RAG2( 2) 

1. ODO 
RBG3( 3) 

> -RBG3( 2) 

; MIC3 - MIC4 

: -1. ODO 
RAG2( 3) 

; -RAG2( 1) 

1. ODO 
-RBG3( 3) 

> RBG3( 1 ) 

MJC3 - MJC4 

-1. ODO 
-RAG2( 2) 

• RAG2( 1 ) 

1. ODO 
RBG3( 2) 

-RBG3( 1 ) 

; MKC3 - MKC4 
EQUATIONS 

■ RBG3( 3) 

' -RBG3( 2) 

' -IXXT( 3) 

' IXYT( 3) 

: IXZT( 3) 

-T2X 

-RBG3( 3) 

■ RBG3( 1) 

• IXYT( 3) 
-IYYT( 3) 

: IYZT( 3) 

-T2Y 

‘ RBG3( 2) 

‘ -RBG3(1) 

= IXZT( 3) 

‘ IYZT( 3) 

• -IZZT(3) 

• -T2Z 



***** FIRST LINK IS CONSTRAINED TO ROTATE IN Z DIRECTION 
590 DO 600 I = 4,8 

DO 595 J = 1,27 



***** 
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* 

595 

600 



605 

610 



■>v 

620 

624 

627 

630 

631 

633 

635 

“V it 
*** 

* 

640 

* 

Vf 



MATA( I , J) = 0. 0D0 
MATA(J,I) = 0. ODO 
CONTINUE 
MATB(I) = 0. ODO 
CONTINUE 

MATA( 4,4) = 1. ODO 
MATA(5,5) = 1. ODO 
MATA( 6,6) = 1. ODO 
MATA( 7 j 7 ) = 1. ODO 
MATA( 8,8) = 1. ODO 



DO 610 J = 1,27 

MATA( 18 ,J) = 0. ODO 
MATA( 27 , J) = 0. ODO 
CONTINUE 



MATA(9 ,9) 
MATA( 18,9) 
MATA( 18,18) 
MATB( 18) 
MATA( 27 , 9 ) 
MATA( 27 , 27 ) 
MATB( 27) 



-( IZZT( 1)+IZZT( 2)+IZZT( 3) ) 
-1. ODO 
1. ODO 

0. ODO 
-1. ODO 

1. ODO 
0. ODO 



CALL EQUATION SOLVER PROGRAM FROM IMSL 

CALL LEQT2F( MATA , M , N , I A , MATB , IDGT, WKAREA , IER) 

IF (IER. EQ. 0) THEN 

GOTO 640 

ELSE 

WRITE (*,624) IER 
FORMAT (15) 

DO 635 I = 1, 27 
WRITE (*,627) I 
FORMAT (17) 

DO 631 J = 1, 27, 3 

WRITE (*,630) J,MATA( I , J) , J+l ,MATA( I , J+l) , J+2,MATA( I , J+2) 
FORMAT ( 15 ,F13. 5 , 15 ,F13. 5 , 15 ,F13. 5) 

CONTINUE 

WRITE (*,633) I , MATB ( I ) 

FORMAT (I3,F15. 5) 

CONTINUE 
CALL ENDJOB 
END IF 



FIND LCOGX , LCOGY , LCOGZ , THETA VALUES ,WX,WY,WZ 
MODIFIED BY R. M. VERBOS 



JOINT ZERO 

FXO = MATB ( 1 ) 

FYO = MATB( 2) 

FZO = MATB( 3) 

LINK ONE 

SINCE LINK1 IS CONSTRAIN NOT TO MOVE ALL ACC AND VEL ARE ZERO 
AX1 = 0. ODO 
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*660 

sV 

y? 

* 

#v 

* 

* 

* 



•>v 

* 

* 

* 



* 

* 

‘V 

vV 
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AY1 = 0.0D0 
AZ1 = 0. ODO 
AX1 = MATES ( 4) 

VELX1 = INTGRL( 0. ,AX1) 
LC0GX1 = INTGRL(X1 , VELX1) 
LCOGX(l) = LC0GX1 
AY1 = MATB( 5 ) 

VELY1 = INTGRL(0. ,AY1) 
LC0GY1 = INTGRL( Y1 , VELY1) 
LCOGY(l) = LC0GY1 
AZ1 = MATB ( 6 ) 

VELZ1 = INTGRL(0. ,AZ1) 
LC0GZ1 = INTGRL( Z1 , VELZ1) 
LCOGZ(l) = LC0GZ1 
WD1X = MATB( 7) 

W1X = INTGRL( 0. ,WD1X) 

WDX(l) = WD1X 

Wl(l) = W1X 

WD1Y = MATB( 8) 

W1Y = INTGRL( 0. ,WD1Y) 

WDY(l) = WD1Y 

W 1 ( 2 ) = W1Y 

WD1Z = MATB ( 9 ) 

W1Z = INTGRL(0. ,WD1Z) 

WDZ(l) = WD1Z 

Wl( 3) = W1Z 

ADDED BY R. M. VERBOS 

TKZ = INTGRL( 0. ,W1Z) 

THZANG = THZ * RADEG 

COSTHZ = DCOS(THZ) 

SINTHZ = DSIN(THZ) 



* IF THE 1ST LINK IS CONSTRAIN TO ROTATE IN THE Z DIRECTION ONLY 

* THE DIRECTION COSINE AND DIRECTION COSINE ANGLES ARE CONSTANT 

* AND DO NOT NEED TO BE CALCULATED 

* CALC DIRECTIONAL COSINES FOR LINK ONE 



*700 

y? 

/» 



DRCSX(l) = LC0GX1 / LNC0G1 

DRCSY(l) = LC0GY1 / LNC0G1 

DRCSZ(l) = LC0GZ1 / LNC0G1 

AMP = DSQRT(DRCSX(1)*DRCSX(1)+DRCSY(1)*DRCSY(1)+. 

DRCSZ( 1)*DRCSZ( 1) ) 

DRCSX(l) = DRCSX( 1)/AMP 
DRCSY(l) = DRCSY( 1 )/AMP 
DRCSZ(l) = DRCSZ( 1) /AMP 



*720 


DRCANX(l) = DACOS( DRCSX( 1) ) 


* RADEG 


* 


DRCANY(l) = DAC0S(DRCSY(1)) 


* RADEG 


* 


DRCANZ(l) = DAC0S(DRCSZ(1)) 


* RADEG 




JOINT LOCATION 




*730 


JX1 = LINKL1 * DRCSX(l) 




* 


JY1 = LINKL1 * DRCSY(l) 






JZ1 = LINKL1 * DRCSZ(l) 
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* 

735 

** 

740 



* 



790 



* 

800 



* 

805 



** 

812 



JOINT ONE 

FX1 = MATB(IO) 
FY1 = MATB(ll) 
FZ1 = MATB( 12) 
LINK TWO 



AX 2 


= 


MATB( 13) 


VELX2 


= 


INTGRL(0. , AX2) 


LC0GX2 


= 


INTGRLC X2 , VELX2 ) 


LCOGX( 2) 




LC0GX2 


AY2 


= 


MATB(14) 


VELY2 


= 


INTGRLCO. , AY2) 


LC0GY2 


= 


INTGRLC Y2,VELY2) 


LC0GY(2) 


= 


LC0GY2 


AZ2 


= 


MATB C 15) 


VELZ2 


= 


INTGRLCO. , AZ2) 


LC0GZ2 


= 


INTGRLC Z2,VELZ2) 


LCOGZ( 2 ) 


= 


LC0GZ2 


WD2X 


= 


MATB C 16) 


W2X 


— 


INTGRLCO. ,WE)2X) 


WDX( 2) 


= 


WD2X 


W2( 1) 


= 


W2X 


WD2Y 




MATBC 17) 


W2Y 


= 


INTGRLCO. ,WD2Y) 


WDY( 2) 


= 


WD2Y 


W2(2) 


= 


W2Y 


WD2Z 


— 


MATBC 18) 


W2Z 


— 


INTGRLCO. ,WD2Z) 


WDZ(2) 


= 


WD2Z 


W2(3) 


= 


W2Z 



GET THE DIRECTION COSINES FOR THE LINK TWO 
DRCSX( 2) = ( LC0GX2 - JX1) / LNC0G2 

DRCSY( 2) = ( LC0GY2 - JY1) / LNC0G2 

DRCSZ( 2) = ( LC0GZ2 - JZ1) / LNC0G2 

AMP = DSQRT(DRCSX(2)*DRCSX(2)+DRCSY(2)*DRCSY(2)+. . 

DRCSZ( 2)*DRCSZ( 2) ) 

DRCSX( 2) = DRCSX(2)/AMP 

DRCSY( 2) = DRCSY(2)/AMP 

DRCSZ( 2) = DRCSZ(2)/AMP 

DRCANX( 2) = DACOS(DRCSX( 2) ) * RADEG 

DRCANY( 2) = DACOS(DRCSY( 2) ) * RADEG 

DRCANZ( 2) = DACOS(DRCSZ( 2) ) * RADEG 

JOINT LOCATION 

JX2 = JX1 + LINKL2 * DRCSX(2) 

JY2 = JY1 + LINKL2 * DRCSY(2) 

JZ2 = JZ1 + LINKL2 * DRCSZ(2) 

JOINT TOO 

FX2 = MATB( 19) 

FY2 = MATB( 20) 

FZ2 = MATB (21) 

LINK THREE 

AX 3 = MATB( 22) 

VELX3 = INTGRLCO. ,AX3) 
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LCOGX3 


= 


INTGRL( X3 , VELX3) 


LCOGX( 3) 


= 


LCOGX3 


AY3 


— 


MATB( 23) 


VELY3 


= 


INTGRL( 0. , AY3) 


LC0GY3 


= 


INTGRL( Y3 , VELY3) 


LCOGY( 3 ) 


= 


LCOGY3 


AZ3 


= 


MATB( 24) 


VELZ3 


= 


INTGRL( 0. , AZ3) 


LCOGZ3 




INTGRL( Z3 , VELZ3) 


LCOGZ( 3) 


= 


LCOGZ3 


WD3X 


— 


MATB( 25) 


W3X 


= 


INTGRL( 0. ,WD3X) 


WDX( 3 ) 


= 


WD3X 


W3( 1) 


= 


W3X 


WD3Y 


= 


MATB( 26) 


W3Y 


— 


INTGRL( 0. ,WD3Y) 


WDY ( 3 ) 


= 


WD3Y 


W3( 2) 


= 


W3Y 


WD3Z 




MATB( 27) 


W3Z 


— 


INTGRL( 0. ,WD3Z) 


WDZ( 3) 


— 


WD3Z 


W 3 ( 3 ) 


= 


W3Z 



* CALC DIRECTIONAL COSINES FOR LINK THREE 

845 DRCSX( 3) = (LCOGX3 - JX2) / LNCOG3 

DRCSY( 3) = ( LCOGY3 - JY2) / LNCOG3 
DRCSZ( 3) = ( LCOGZ3 - JZ2) / LNCOG3 
865 AMP = DSQRT(DRCSX(3)*DRCSX(3)+DRCSY(3)*DRCSY(3)+. . . 

DRCSZ( 3)*DRCSZ( 3) ) 

DRCSX( 3) = DRCSX( 3)/AMP 

DRCSY( 3) = DRCSY( 3) /AMP 

DRCSZ( 3) = DRCSZ( 3) /AMP 

DRCANX( 3) = DACOS(DRCSX( 3) ) * RADEG 

DRCANY( 3) = DACOS(DRCSY( 3) ) * RADEG 

DRCANZ( 3) = DACOS(DRCSZ( 3) ) * RADEG 

* TIP LOCATION 

*875 TIPX = JX2 + LINKL3 * DRCSX(3) 

* TIPY = JY2 + LINKL3 * DRCSY(3) 

* TIPZ = JZ2 + LINKL3 * DRCSZ(3) 

* FIND THE ANGLE BETWEEN THE LIMKS 

880 ANG23X = DRCANX(2) - DRCANX(3) 

ANG23Y = DRCANY( 2) - DRCANY( 3) 

ANG23Z = DRCANZ( 2) - DRCANZ(3) 

ANG23 = ANG23X + ANG23Y + ANG23Z 



* ANG12X = DRCANX(l) - DRCANX(2) 

* ANG12Y = DRCANY(l) - DRCANY( 2) 

* ANG12Z = DRCANZ(l) - DRCANZ(2) 

* DOUBLE PENDULUM CALCULATION 

PA = L3*M3P*SIN(TH2-TH3)*TH3DOT**2 

PB = SIN(TH2)*G*( M3P+M2P) 

PC = L3*M3P*COS( TH2 -TH3 ) 

PD = L2*(M3P+M2P) 
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PE = L2*SIN(TH2-TH3)*TH2D0T**2 
PF = SIN(TH3)*G 
PG = C0S(TH2-TH3)*L2 
PH = L3 

TH3DDT = ( PE-PF+( ( PG*PA+PG*PB) /PD) ) /( PH-( PG*PC/PD) ) 
TH2DDT = (-PA - PB - PC*TH3DDT)/PD 
TH3D0T = INTGRL(TH3D0 ,TH3DDT) 

TH2D0T = INTGRL(TH2D0 ,TH2DDT) 

TH3 = INTGRL(TH30 ,TH3D0T) 

TH2 = INTGRL(TH20,TH2DOT) 

TH2ANG = TH2 * RADEG 
IF(TH2ANG. GT. 0. 0)THEN 
ANGTH2 = 180.0 - DRCANZ(2) 

ELSE 

ANGTH2 = DRCANZ( 2) - 180.0 
ENDIF 

TH3ANG = TH3 * RADEG 
IF(TH3ANG. GT. 0. 0)THEN 
ANGTH3 = 180.0 - DRCANZ(3) 

ELSE 

ANGTH3 = DRCANZ( 3) - 180.0 
ENDIF 



END 

STOP 

FORTRAN 

* SUBROUTINE TO COMPUTE THE CROSS PRODUCT OF TWO VECTORS 

SUBROUTINE CPROD( VECTA , VECTB , MI , MJ , MK) 

940 IMPLICIT REAL* 8 (A-Z) 

DIMENSION VECTA ( 3 ) , VECTB ( 3 ) 

MI = VECTA( 2) * VECTB ( 3) - VECTA(3) * VECTB(2) 

MJ = VECTA( 3) * VECTB ( 1 ) - VECTA(l) * VECTB(3) 

MK = VECTA(l) * VECTB( 2) - VECTA(2) * VECTB(l) 

RETURN 

END 
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APPENDIX C 



THREE DIMENSION VALIDATION PROGRAM 



TITLE NONSINGULAR 3-D VALIDATION PROGRAM 






* THIS IS A PROGRAM WHICH VALIDATES THE NON- SINGULARITY OF A THREE * 

* LINK RIGID BODY MANIPULATOR WITHOUT THE USE OF THE STANDARD ROBOT * 

* TRANSFORMATION MATRICES. THE ORIGINAL PROGRAM WAS WRITTEN BY LT * 

* ATINOK AND HAS BEEN GREATLY MODIFIED TO INCLUDE THREE DIMENSIONAL * 

* MOTION AND GRAVITY BY LT ROBERT M. VERBOS. SEPTEMBER 1988 * 



JLrJ# J* «JLi mJ+ mJ+ wJ- mX* m3 m mJ m mXm J# JU J« J* J# J«J 

4 \ 4\ 4% 4 % 4 \ 4 \ 4% 4% 4% 4% 4% 4% 4% 4% 4% 4% 4% 4 4% 4* #% 4% 4% 4% 4\ 4% 4 \ 4 \ / W 4% 4% 4% 4% 4% 4 % / \ J * 4% 4 % 4 \ 4% 4% 4 % M 4% TV 4*4% TV 4\ 4% 4 \ 4 \ 4 \ 4% 4*4% 4 



^ JL J# 

% 4% 4% 4% 4% 4% 4% 4% 4% 4% 4*1% 



TERMINAL 
METHOD ADAMS 

PRINT . 02, ERROR 1 ,ERROR2 ,ERROR3 , ANG12 , ANG23 

CONTROL FINTIM =1.6, DELT = .0005, DELMAX = .1, DELPRT = .02 

D DIMENSION MATA(27,27) ,MASS(3,2) ,L(3 ,2) ,RX(3,2) ,RY(3,2) ,RZ(3,2) 

D DIMENSION IXX( 3 , 2) , IXZ( 3 , 2) , IXY( 3 ,2) , IYY( 3 , 2) , IYZ( 3 , 2) , IZZ( 3 , 2) 

D DIMENSION IMAT( 3,3,3), NIMAT( 3,3) ,TMAT( 3,3) ,TMATTR( 3,3) ,MATTMP( 3,3) 

D DIMENSION LIMAT(3,3,3) 

FIXED IER , I , J,M ,K,P ,N, IA, IDGT, A, I 1 

ARRAY MATB (27), MATC (27), MATD( 2 7 ) , LCOGX( 3 ) , LCOGY( 3 ) , LCOGZ ( 3 ) 

ARRAY VECTA( 3) , VECTB ( 3 ) 

ARRAY WDX( 3) ,WDY(3) ,WDZ(3) ,W1(3) ,W2(3) ,W3(3) 

ARRAY RBG1( 3) ,RAG1( 3) ,RBG2( 3) ,RAG2(3) ,RBG3(3) 

ARRAY WKAREA( 850) 

ARRAY IXXT( 3 ) , I YYT( 3) , IZZT( 3) , IXYT( 3 ) , IXZT( 3 ) , I YZT( 3 ) 

ARRAY DRCANX( 3) ,DRCANY( 3) ,DRCANZ(3) 

ARRAY DRCSX( 3) ,DRCSY(3) ,DRCSZ( 3) , 

INITIAL 

t ktot IT 1 

4% 4% 4% 4% 4% 4* 4% 4% 4% 4% 

* INPUT PARAMETER CONSTANTS 

55 A 15. 0D0 

P =0. 0D0 
W = PI/2.0 

IDGT = 6 

G = 9.81D0 

N = 27 > 

M = 1 

IA = 27 

IER = 0 

LEVELQ = 0 
LEVLDQ = 0 

* INPUT JOINT LOCATIONS IN METERS 

JX0 = 0. 0D0 
JYO = 0. 0D0 
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JZO = 0. ODO 
JX1 = 0. ODO 
JY1 = 0. ODO 
JZ1 = 0. 2D0 
JX2 = 0. ODO 
JY2 = 0. 416D0 
JZ2 = 0. 2D0 

* INPUT DISTANCE FROM CENTER OF LINK TO CENTER OF MASS 

L( 1 , 1 ) = 0. 05D0 
L( 1,2) = 0. 15D0 
L( 2 , 1) = 0. 208D0 
L( 2 , 2) = 0. 208D0 
L( 3 , 1) = 0. 2255D0 
L( 3 , 2) = 0. 2255D0 

* INPUT THE LINK LENGTHS 

LINKL1 = 0. 20D0 
LINKL2 = 0.416D0 
LINKL3 = 0.451D0 

* INPUT MASS AT LINK ENDS IN KILOGRAMS 

83 MASS( 1,1) = 1. ODO 

MASS( 1,2) = 3. ODO 
MASS( 2,1) = 2.2D0 
MASS( 2,2) = 2. 2D0 
MASS( 3 , 1) = 8.6D0 
MASS( 3,2) = 8. 6D0 

* INPUT LOCATION OF LINK CENTERS OF GRAVITY 



LCOGX(l) 


— 


0. ODO 


XI 


= 


LCOGX(l) 


LCOGY(l) 


— 


0. ODO 


Y1 


— 


LCOGY(l) 


LCOGZ(l) 


= 


0. 10D0 


Z1 


= 


LCOGZ(l) 


LCOGX( 2) 


= 


0. ODO 


X2 


= 


LCOGX( 2) 


LCOGY( 2) 


— 


0. 208D0 


Y2 


= 


LCOGY( 2) 


LCOGZ( 2) 


= 


0. 20D0 


Z2 


= 


LCOGZC 2) 


LCOGX( 3) 


— 


0. ODO 


X3 


= 


LC0GX(3) 


LCOGY( 3) 


= 


0. 6415D0 


Y3 


= 


LCOGY ( 3 ) 


LC0GZ(3) 


= 


0. 2D0 


Z3 


= 


LCOGZ( 3) 



* INPUT MASS OF EACH LINK IN KG 

Ml = 4. ODO 
M2 = 4. 4D0 
M3 = 17. 2D0 

* INPUT ACCELERATIONS OF JOINT ZERO 

AXO = 0. ODO 
AYO = 0. ODO 
AZO = 0. ODO 

* INPUT THE INITIAL DIRECTION COSINES 

DRCSX( 1) = 0. ODO 
DRCSY( 1) = 0. ODO 
DRCSZ(l) = 1. ODO 
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DRCSX( 2) 
DRCSY( 2) 
DRCSZ( 2) 
DRCSX( 3) 
DRCSY( 3) 
DRCSZ( 3) 



0. 0D0 

1. ODO 
0. ODO 

0. ODO 

1. ODO 
0. ODO 



* 



INPUT THE INITIAL DIRECTION COSINE ANGLES 



DRCANX(l) 
DRCANY(l) 
DRCANZ(l) 
DRCANX( 2) 
DRCANY( 2) 
DRCANZ(2) 
DRCANX( 3) 
DRCANY(3) 
DRCANZ( 3) 



90. ODO 
90. ODO 
0. ODO 
90. ODO 
0. ODO 
90. ODO 
90. ODO 
0. ODO 
90. ODO 



***** 

* 

139 



146 



INITIALIZE ***** 
OMEGA AND OMEGA DOT 
DO 146 I = 1,3 
W 1 ( I ) = 0. ODO 

W2( I) = 0. ODO 
W3( I) = 0. ODO 
WDX(I) = 0. ODO 
WDY(I) = 0. ODO 
WDZ(I) = 0. ODO 
CONTINUE 

W 1 ( 3 ) = 2. ODO 

W 2 ( 1 ) = 2. ODO 

W3( 1) = 2. ODO 

WDY ( 2 ) = 4. ODO 
WDY( 3) = 4. ODO 



THZ = 0. ODO 

* INITIALIZE MATRICES TO ZERO 
DO 180 I = 1,27 
DO 175 J = 1,27 

MATA( I , J) = 0. ODO 
175 CONTINUE 





MATB(I) = 


0. ODO 




MATCH) = 


0. ODO 




MATDH) = 


0. ODO 


180 


CONTINUE 




***** 


CALCULATIONS 


***** 


* 


WEIGHTS (NEWTONS) 




185 


WG1 = M1*G 





WG2 = M2*G 
WG3 = M3*G 
COMPUTE THE LENGTH 
LNC0G1 = DSQRT 

LX2 = LC0GX(2) 
LY2 = LCOGY( 2) 
LZ2 = LCOGZ( 2) 
LNC0G2 = DSQRT 



FROM THE INBOARD JOINT TO COG 
( LCOGX( 1)*LC0GX( 1) + LCOGY( 1)*LC0GY( 1) +. 
LCOGZ( 1)*LC0GZ( 1) ) 

- JX1 

- JY1 

- JZ1 

( LX2--LX2 + LY2*LY2 + LZ2*LZ2 ) 
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* 

190 



200 



205 



LX 3 = LC0GX( 3 ) - 
LY3 = LC0GY( 3) - 
LZ3 = LC0GZ( 3) - 
LNC0G3 = DSQRT ( 
COMPUTE FBD INERTIAS 



JX2 

JY2 

JZ2 

LX 3* LX 3 + LY3*LY3 + LZ3*LZ3 ) 

BASED ON POINT MASSES IN GLOBAL COORDINATES 



DO 



1,3 



"L( 1,1) 
L( I , 2) 
-L(I,1) 
L( I , 2) 
-L(I,1) 
L( I , 2) 

•jV 



MASS( 1,1) * ( (RY( 1,1) 
MASS( 1,2) * ( ( RY( I , 2 ) 
IXX(I,1) + IXX(I,2) 
MASS( 1,1) * ( (RX( 1,1) 
MASS( 1,2) * ( (RX( 1,2) 
IYY( 1,1) + IYY( 1,2) 
MASS( 1,1) * ( (RX( 1,1) 
MASS( 1,2) * ( (RX( 1,2) 
IZZ(I,1) + IZZ( 1,2) 
MASS( 1,1) * RX( 1,1) * 
MASS( 1,2) * RX( I j 2) * 
IXY( 1,1) + IXY( 1,2) 
MASS( 1,1) * RZ( 1,1) * 
MASS( 1,2) * RZ( I , 2) 
IXZ(I,1) + IXZ( 1,2) 
MASS( 1,1) * RY( 1,1) 
MASS( 1,2) * RY( I , 2) 
IYZ( 1,1) + IYZ( 1,2) 
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225 I = 

RX(i,i) 

RX( 1,2) 

RY(I,1) 

RY(I,2) 

R2( I , 1) 

RZ(I,2) 

IXX(I,1) 

IXX( 1,2) 

IXXT(I) 

IYY( 1,1) 

IYY( 1,2) 

IYYT(I) 

IZZ( 1,1) 

I Z Z ( 1,2) 

IZZT(I) 

IXY( 1,1) 

IXY( I j 2) 

IXYT(I) 

IXZ(I,1) 

IXZ( 1,2) 

IXZT(I) 

IYZ( 1,1) 

IYZ( 1,2) 

IYZT(I) 

IF (IXXT(I) .LE. 

IXXT(I) = . 01 
ELSE 

IXXT(I) = IXXT(I) 

END IF 

IF (IYYT(I) .LE. 

IYYT(I) = . 01 
ELSE 

IYYT(I) = IYYT(I) 

END IF 

IF (IZZT(I) . LE. . 01) THEN 

IZZT(I) = .01 

ELSE 

IZZT(I) = IZZT(I) 

END IF 

IMAT( 1,1,1) 

IMAT( 1,1,2) 

IMAT( 1,1,3) 

I MAT (1,2,1) 

IMAT( 1,2,2) 

IMAT( 1,2,3) 

IMAT( 1,3,1) 

IMAT( 1,3,2) 

IMAT( 1,3,3) 

CONTINUE 

DUE TO LINK 1 CONSTRAINTS 



* 

* 

* 

* 



DRCSX(I) 

DRCSX(I) 

DRCSY(I) 

DRCSY(I) 

DRCSZ(I) 

DRCSZ(I) 

* RY( I , 1) ) 
RY ( I , 2 ) ) 



* 



01) THEN 



01) THEN 



IXXT( I ) 
IXYT( I ) 
IXZT(I) 
-IXYT(I) 
IYYT(I) 
IYZT(I) 
-IXZT(I) 
-IYZT(I) 
IZZT(I) 



* 

* 

* 

* 



RX( 1,1)) 
RX(I,2)) 

RX(I,1)) 
RX( 1,2)) 



(RZ(I ,1) 
(RZ( 1,2) 

(RZ(I,1) 

(RZ(I,2) 

(RY(I,1) 
( RY( I , 2 ) 



* 

* 

* 

* 

* 



RZ(i,i))) 
RZ( I , 2) ) ) 

RZ( 1 , 1)) ) 
RZ( 1,2))) 

RY( I , 1 ) ) ) 
RY ( I , 2 ) ) ) 



RY(I,1) 
RY( I , 2) 

RX( 1,1) 
RX( 1,2) 

RZ( 1,1) 
RZ( I j 2) 



AND SYMMETRY LINK 1 INERTIAS ARE CONST 
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■jV 



270 

272 



278 

280 

281 



IXXT(l) = 
IXYT(l) = 
IXZT(l) = 
IYYT(l) = 
IYZT(l) = 
IZZT(l) = 
TRANSFORM THE 

DO 281 1=2, 
TMAT( 2,1) = 
TMAT( 2 , 2) = 
TMAT( 2,3) = 
TMAT( 3,1) = 
TMAT( 3,2) = 
TMAT( 3,3) = 
VECTA(l) = 
VECTA( 2) = 

VECTA( 3) = 

VECTB(l) = 
VECTB( 2) = 

VECTB( 3) = 

CALL CPROD 



IMAT( 1,1,1) 

IMAT( 1,1,2) 

IMAT( 1,1,3) 

IMAT( 1,2,2) 

IMAT( 1,2,3) 

IMAT( 1,3,3) 

GLOBAL INERTIAS TO LOCAL WHICH REMAIN CONSTANT 
3 

-DCOS ( THZ ) 

-DSIN ( THZ ) 

0. 0D0 
DRCSX(I) 

DRCSY(I) 

DRCSZ(I) 

TMAT( 2,1) 

TMAT( 2,2) 

TMAT( 2,3) 

TM AT (3,1) 

TMAT (3,2) 

TMAT( 3,3) 

( VECTA , VECTB , M I 1 , M J 1 , MK 1 ) 



TMAT( 1,1) = 
TMAT( 1,2) = 
TMAT( 1,3) = 
TMATTRC 1,1) 
TMATTRC 1,2) 
TMATTRC 1,3) 
TMATTR( 2, 1) 
TMATTR( 2, 2) 
TMATTRC 2, 3) 
TMATTRC 3,1) 
TMATTRC 3, 2) 
TMATTRC 3,3) 
DO 272 II = 
DO 272 J = 
TEMP = 0. 



Mil 

MJ1 

MK1 

= TMATC 1,1) 
= TMAT( 2,1) 
= TMATC 3,1) 
= TMATC 1,2) 
= TMATC 2, 2) 
= TMATC 3, 2) 
= TMATC 1,3) 
= TMATC 2, 3) 
= TMATC 3,3) 
1,3 
1,3 
ODO 



DO 270 K =1,3 

TEMP = TMATCI1,K) * IMATCI,K,J) + TEMP 
CONTINUE 

MATTMPC II , J) = TEMP 
CONTINUE 

DO 280 II = 1,3 

DO 280 J =1,3 

TEMP = 0. ODO 
DO 278 K = 1,3 

TEMP = MATTMPC I 1,K) * TMATTRC K,J) + TEMP 
CONTINUE 

LIMATC I , II , J) = TEMP 
CONTINUE 
CONTINUE 



DERIVATIVE 

NOSORT 

287 CALL ERRSET C 208 , 256 , - 1 , 1 , 1) 

CALL UERSETC LEVELQ , LEVLDQ) 

* INITIALIZE MATRICES TO ZERO 
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DO 297 I = 1,27 
DO 293 J = 1,27 

MATA( I , J) = 0. ODO 
293 CONTINUE 

MATB(I) = 0. ODO 
MATC(I) = 0. ODO 
MATD(I) = 0. ODO 
297 CONTINUE 



* 

* 

* 



VALIDATION PORTION OF PROGRAM *** 

ENTER THE ANGULAR AND LINEAR ACCELERATIONS, AND FORCES INTO MATRIX C 
FOR THE CONSTRAIN OF THE NEPTUNE II 



TH1L 

W1L 

WD1L 

TH1ANG 

WDX1G 

WDY1G 

WDZ1G 

WX1G 

WY1G 

WZ1G 

TH2L 

W2L 

WD2L 

TH2ANG 

WX2G 

WY2G 

WZ2G 

VECTA(l) = 

VECTA( 2) = 

VECTA( 3) = 

VECTB(l) = 

VECTB( 2) = 

VECTB( 3) = 

CALL CPROD 

WDX2G 

WDY2G 

WDZ2G 

TH3L 

W3L 

WD3L 

TH3ANG 

WX3G 

WY3G 

WZ3G 

WDX3G 

WDY3G 

WDZ3G 

R2X 

R2Y 

R2Z 

RJ2X 

RJ2Y 

RJ2Z 

R3X 



2.0 * TIME + PI/2.0 
2. ODO 
0. ODO 

TH1L * RADEG 

0. ODO 

0. ODO 

WD1L 

0. ODO 

0. ODO 

W1L 

2. 0 * TIME 
2 . 0 
0. ODO 

TH2L * RADEG 
W2L * SIN(THIL) 

(- COS(THIL) 



) 



W2L * 

W1L 
0. ODO 
0. ODO 
WZ2G 
WX2G 
WY2G 
0. ODO 

( VECTA , VECTB , MI 1 , MJ1 , MK1 ) 
Mil 
MJ1 
MK1 
0. ODO 
0. ODO 
0. ODO 
TH3L * 

WX2G 
WY2G 
WZ2G 
WDX2G 
WDY2G 
WDZ2G 
LNC0G2 
LNC0G2 
LNC0G2 
LINKL2 
LINKL2 
LINKL2 
RJ2X + 



RADEG 



* C0S(TH2L) 

* C0S(TH2L) 

* SIN(TH2L) 

* C0S(TH2L) 

* C0S(TH2L) 

* SIN(TH2L) 

LNC0G3 * COS(TH2L+TH3L) * COS(THIL) 



* 



COS(THIL) 

SIN(THIL) 

COS(THIL) 

SIN(THIL) 
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R3Y = RJ2Y + LNC0G3 * C0S(TH2L+TH3L) * SIN(THIL) 

R3Z = RJ2Z + LNC0G3 * SIN(TH2L+TH3L) 

LTIPX = RJ2X + LINKL3 * C0S(TH2L+TH3L) * COS(THIL) 

LTIPY = RJ2Y + LINKL3 * COS(TH2L+TH3L) * SIN(THIL) 

LTIPZ = RJ2Z + LINKL3 * SIN(TH2L+TH3L) + JZ1 

CALCULATE THE GLOBAL ACCELERATIONS 
AX1G = 0. 0D0 

AY1G = 0. 0D0 

AZ1G = 0. ODO 

VECTA(l) = WDX2G 
VECTA( 2) = WDY2G 
VECTA( 3) = WDZ2G 
VECTB(l) = R2X 
VECTB( 2) = R2Y 
VECTB( 3) = R2Z 

CALL CPROD ( VECTA , VECTB , MI 1 , M J 1 , MK1 ) 

VECTA(l) = WX2G 
VECTA( 2) = WY2G 
VECTA( 3) = WZ2G 

CALL CPROD (VECTA, VECTB, MI 2, MJ2,MK2) 

VECTB(l) = MI2 
VECTB ( 2) = MJ2 
VECTB ( 3) = MK2 

CALL CPROD (VECTA, VECTB, MI 2, MJ2,MK2) 

AX2G = Mil + MI2 
AY2G = MJ1 + MJ2 
AZ2G = MK1 + MK2 
VECTA(l) = WDX3G 
VECTA( 2) = WDY3G 
VECTA( 3) = WDZ3G 
VECTB(l) = R3X 
VECTB ( 2) = R3Y 
VECTB( 3) = R3Z 

CALL CPROD (VECTA, VECTB, MI 1,MJ1,MK1) 

VECTA(l) = WX3G 
VECTA ( 2) = WY3G 
VECTA( 3) = WZ3G 

CALL CPROD (VECTA, VECTB, MI 2, MJ2,MK2) 

VECTB ( 1) = MI 2 
VECTB ( 2) = MJ2 
VECTB( 3) = MK2 

CALL CPROD (VECTA, VECTB, MI 2, MJ2,MK2) 



AX3G 


= Mil + MI2 


AY3G 


= MJ1 + MJ2 


AZ3G 


= MK1 + MK2 


SET MATRIX 


D 


MATD( 27) 


= WDZ3G 


MATD( 26) 


= WDY3G 


MATD( 25) 


= WDX3G 


MATD( 24) 


= AZ3G 


MATD( 23) 


= AY3G 


MATD( 22) 


= AX3G 


MATD( 21) 


= -M3 * AZ3G 


MATD( 20) 


= -M3 * AY3G 


MATD( 19) 


= -M3 * AX3G 


MATD( 18) 


= WDZ2G 
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MATD( 17) 


= 


VDY2G 








MATD( 16) 


= 


VDX2G 








MATD( 15 ) 


= 


AZ2G 








MATD( 14) 


= 


AY2G 








MATD( 13) 


= 


AX2G 








MATD( 12) 


= 


MATD( 21) 


- M2 


* 


AZ2G 


MATD( 11) 


— 


MATD( 20) 


- M2 


* 


AY2G 


MATD( 10) 


= 


MATD( 19) 


- M2 


* 


AX2G 


MATD( 9) 


= 


VDZ1G 








MATD( 8) 




VDY1G 








MATD( 7 ) 


= 


VDX1G 








MATD( 6) 




AZ1G 








MATD( 5 ) 


= 


AY1G 








MATD( 4) 




AX1G 








MATD( 3) 


= 


MATD( 12) 


- Ml 


* 


AZ1G 


MATD( 2) 


= 


MATD(ll) 


- Ml 


* 


AY1G 


MATD( 1) 


= 


MATD(IO) 


- Ml 


* 


AX1G 



*** DIRECT DYNAMICS PORTION OF PROGRAM *** 

* INPUT JOINT EQUATIONS 

* JOINT ZERO EQUATIONS 

* VI X (VI X RB/G1) 

RBG1(1) = JXO - LCOGX(l) 

RBG1(2) = JYO - LCOGY(l) 

RBG1(3) = JZO - LCOGZ(l) 

VECTA(l) = Vl(l) 

VECTA( 2) = V 1 ( 2 ) 

VECTA(3) = Vl( 3) 

CALL CPROD( VECTA ,RBG1 , MICO , MJCO ,MKC0) 
VECTB(l) = MICO 
VECTB( 2) = MJCO 
VECTB( 3) = MKCO 

CALL CPROD( VECTA, VECTB, MICO, MJCO, MKCO) 

* VI X (VI X RA/G1) 

RAG1(1) = JX1 - LCOGX(l) 

RAG1( 2) = JY1 - LCOGY(l) 

RAG1( 3) = JZ1 - LCOGZ(l) 

VECTA(l) = Vl(l) 

VECTA( 2) = V 1 ( 2 ) 

VECTA( 3) = Vl( 3) 

CALL CPROD ( VECTA, RAG 1, MI Cl ,MJC1 ,MKC1) 
VECTB ( 1) = MIC1 
VECTB ( 2) = MJC1 
VECTB( 3) = MKC1 

CALL CPROD (VECTA, VECTB, MIC 1,MJC1,MKC1) 

* V2 X ( V2 X RB/G2) 

RBG2( 1) = JX1 - LCOGX( 2) 

RBG2( 2) = JY1 - LCOGY( 2) 

RBG2( 3) = JZ1 - LCOGZ( 2) 

VECTA(l) = V2( 1) 

VECTA( 2) = V2( 2) 

VECTA( 3) = V2( 3) 

CALL CPROD (VECTA, RBG2 ,MIC2 ,MJC2 ,MKC2) 
VECTB ( 1) = MIC2 
VECTB ( 2) = MJC2 
VECTB( 3) = MKC2 
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CALL CPROD ( VECTA , VECTB , MIC2 , MJC2 , MKC2) 
W2 X ( W2 X RA/G2) 

RAG2( 1) = JX2 - LCOGX( 2) 

RAG2( 2) = JY2 - LCOGY(2) 

RAG2( 3) = JZ2 - LCOGZ( 2) 

VECTA(l) = W2( 1 ) 

VECTA( 2) = W2( 2) 

VECTA( 3) = W2( 3) 

CALL CPROD (VECTA, RAG2 ,MIC3 ,MJC3 ,MKC3) 
VECTB(l) = MIC3 
VECTB( 2) = MJC3 
VECTB( 3) = MKC3 

CALL CPROD( VECTA , VECTB , MIC3 , MJC3 , MKC3) 
W3 X (W3 X RB/G3) 

RBG3(1) = JX2 - LCOGX( 3) 

RBG3( 2) = JY2 - LC0GY(3) 

RBG3( 3) = JZ2 - LCOGZ( 3) 

VECTA(l) = W3C 1) 

VECTA ( 2) = W3( 2) 

VECTA( 3) = W3( 3) 

CALL CPROD (VECTA, RBG3,MIC4,MJC4,MKC4) 
VECTB(l) = MIC4 
VECTB (2) = MJC4 
VECTB( 3) = MKC4 

CALL CPROD (VECTA, VECTB, MIC4,MJC4 ,MKC4) 
INERTIA TRANSFORMATION 

TMAT( 2,1) = -DCOS ( THZ ) 

TMAT( 2,2) = -DSIN ( THZ ) 

TMAT( 2,3) = 0. ODO 
DO 880 I = 2, 3 

TMAT( 3,1) = DRCSX(I) 

TMAT( 3,2) = DRCSY(I) 

TMAT( 3,3) = DRCSZ(I) 

VECTA(l) = TMAT( 2,1) 

VECTA( 2) = TMAT( 2,2) 

VECTA( 3) = TMAT( 2,3) 

VECTB(l) = TMAT(3,1) 

VECTB( 2) = TMAT( 3,2) 

VECTB( 3) = TMAT (3,3) 

CALL CPROD ( VECTA, VECTB, MI 1,MJ1,MK1) 

TMAT( 1,1) = Mil 

TMAT (1,2) = MJ1 

TMAT( 1,3) = MK1 

TMATTR( 1,1) = TMAT( 1,1) 

TMATTR( 1,2) = TMAT(2,1) 

TMATTR( 1,3) = TMAT(3,1) 

TMATTR( 2,1) = TMAT( 1,2) 

TMATTR(2,2) = TMAT(2,2) 

TMATTR(2,3) = TMAT(3,2) 

TMATTR( 3,1) = TMAT(1,3) 

TMATTR( 3,2) = TMAT(2,3) 

TMATTR( 3,3) = TMAT(3,3) 

DO 866 II = 1,3 
DO 866 J = 1,3 
TEMP = 0. ODO 
DO 864 K = 1,3 
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TEMP = TMATTR( I 1 ,K) * LIMAT(I,K,J) + TEMP 
864 CONTINUE 

MATTMP( I 1 , J) = TEMP 
866 CONTINUE 

DO 874 II = 1,3 
DO 874 J = 1,3 
TEMP = 0. ODO 
DO 872 K = 1,3 

TEMP = MATTMP( 1 1 ,K) * TMAT(K, J) + TEMP 
872 CONTINUE 

NIMAT( I 1 , J) = TEMP 



874 


CONTINUE 








IXXT(I) 


= 


NIMAT( 1,1) 




IXYT(I) 


= 


NIMAT( 1,2) 




IXZT(I) 


- 


NIMAT( 1,3) 




IYYT(I) 


= 


NIMAT( 2,2) 




IYZT(I) 


— 


NIMAT( 2,3) 




IZZT(I) 


= 


NIMAT( 3,3) 


880 


CONTINUE 






* 


ENTER CONSTANS 


INTO MATRIX A 


** 


LINK ONE 






* 


SUM OF FORCES 




V: 


X DIRECTION 
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MATA( 1,1) 


= 


1. ODO 




MATA( 1,4) 


= 


Ml 




MATA( 1,10) 


= 


-1. ODO 


* 


Y DIRECTION 








MATA( 2,2) 




1. ODO 




MATA( 2,5) 


— 


Ml 




MATA(2, 11) 


= 


-1. ODO 




Z DIRECTION 








MATA( 3,3) 


= 


1. ODO 




MATA( 3,6) 


= 


Ml 




MATA(3, 12) 


— 


-1. ODO 


Vc 


EQUATIONS AT 


JOINT ZERO 


* 


X DIRECTION 








MATA(4,4) < 




1. ODO 




MATA(4,8) ! 




RBG1( 3) 




MATA( 4,9) > 




-RBG1(2) 


* 


Y DIRECTION 








MATA(5 ,5) ■ 




1. ODO 




MATA(5 , 7) = 




■RBG1( 3) 




MATA( 5,9) = 




RBG1(1) 


* 


Z DIRECTION 








MATA( 6,6) * 




1. ODO 




MATA( 6,7) i 




RBG1( 2) 




MATA( 6,8) = 




-RBGl(l) 


* 


SUM OF MOMENTS 


EQUATIONS 


mSm 


X DIRECTION 








MATA( 7,2) 


= 


RBG1( 3) 




MATA( 7,3) 


— 


-RBG1(2) 




MATA( 7,7) 


= 


-IXXT(l) 




MATA( 7,8) 


= 


IXYT(l) 




MATA( 7,9) 


= 


IXZT(l) 




MATA( 7,11) 


= 


-RAG1(3) 
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MATA( 7,12) 


= 


RAG1( 2) 


Y DIRECTION 


MATA( 8,1) 


= 


-RBG1(3) 


MATA( 8,3) 


= 


RBGl(l) 


MATA( 8,7) 


= 


IXYT(l) 


MATA( 8,8) 


= 


-IYYT(l) 


MATA( 8,9) 




IYZT(l) 


MATA( 8,10) 


= 


RAG1( 3) 


MATA( 8,12) 




-RAGl(l) 


Z DIRECTION 


MATA( 9,1) 


= 


RBG1( 2) 


MATA( 9,2) 


= 


-RBGl(l) 


MATA( 9,7) 




IXZT(l) 


MATA( 9,8) 


— 


IYZT(l) 


MATA( 9,9) 


= 


-IZZT(l) 


MATA( 9,10) 


= 


-RAG1(2) 


MATA(9, 11) 




RAGl(l) 





LINK TWO 




* 


SUM OF FORCES 




* 


X DIRECTION 




939 


MATA( 10,10) 


= 1.0D0 




MATA( 10,13) 


= M2 




MATA( 10,19) 


= -1.0D0 


?v 


Y DIRECTION 






MATA( 11,11) 


= 1. 0D0 




MATA( 11,14) 


= M2 




MATA( 11,20) 


= -1.0D0 


Vc 


Z DIRECTION 






MATA( 12,12) 


= 1.0D0 




MATA( 12,15) 


= M2 




MATA( 12,21) 


= -1.0D0 


* 


EQUATIONS AT JOINT ONE 


Vc 


X DIRECTION 






MATA( 13,4) 


= -l.ODO 




MATA( 13,8) 


= -RAG1( 3) 




MATA( 13,9) 


= RAG1( 2) 




MATA( 13,13) 


= 1. ODO 




MATA( 13,17) 


= RBG2( 3) 




MATA( 13,18) 


= -RBG2( 2) 


* 


Y DIRECTION 






MATA( 14,5) 


= -l.ODO 




MATA( 14,7) 


= RAG1(3) 




MATA( 14,9) 


= -RAGl(l) 




MATA( 14,14”) 


= l.ODO 




MATA( 14,16 ) 


= -RBG2( 3) 




MATA( 14,18) 


= RBG2( 1) 


* 


Z DIRECTION 






MATA( 15,6) 


= -l.ODO 




MATA( 15,7) 


= -RAG1(2) 




MATA( 15,8) 


= RAG1(1) 




MATA( 15,15) 


= l.ODO 




MATA( 15,16) 


= RBG2( 2) 




MATA( 15,17) 


= -RBG2( 1 ) 


* 


SUM OF MOMENTS EQUATIONS 


■>v 


X DIRECTION 






MATA( 16,11) 


= RBG2( 3) 
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MATA( 16,12) 
MATA( 16,16) 
MATAC 16,17) 
MATA( 16,18) 
MATAC 16,20) 
MATA( 16,21) 

* Y DIRECTION 

MATA( 17, 10) 
MATA( 17, 12) 
MATAC 17, 16) 
MATAC 17, 17) 
MATAC 17, 18) 
MATAC 17, 19) 
MATAC 17,21) 

* Z DIRECTION 

MATAC 18, 10) 
MATAC 18, 11) 
MATAC 18, 16) 
MATAC 18,17) 
MATAC 18, 18) 
MATAC 18, 19) 
MATAC 18,20) 
** LINK THREE 

* SUM OF FORCES 

* X DIRECTION 
1000 MATAC 19, 19) 

MATAC 19, 22) 

* Y DIRECTION 

MATAC 20, 20) 
MATAC 20, 23) 

* Z DIRECTION 

MATAC 21, 21) 
MATAC 2 1,24) 

* EQUATIONS AT 

* X DIRECTION 

MATAC 22, 13) 
MATAC 22, 17) 
MATAC 22, 18) 
MATAC 22, 22) 
MATAC 22, 26) 
MATAC 22, 27) 

* Y DIRECTION 

MATAC 23, 14) 
MATAC 23, 16) 
MATAC 23, 18) 
MATAC 23, 23) 
MATAC 23, 25) 
MATAC 23, 27) 

* Z DIRECTION 

MATAC 24, 15) 
MATAC 24, 16) 
MATAC 24, 17) 
MATAC 24, 24) 
MATAC 24, 25) 



-RBG2C 2) 
-IXXTC2) 
IXYT(2) 
IXZTC2) 
-RAG2( 3) 
RAG2C2) 

-RBG2C3) 
RBG2C 1) 
IXYTC2) 
-IYYT(2) 
IYZT( 2) 
RAG2C 3) 
-RAG2C1) 

RBG2C2) 
-RBG2C1) 
IXZTC2) 
IYZT(2) 
•IZZTC2) 
•RAG2C 2) 
RAG2(1) 



= 1. ODO 

= M3 

= 1. ODO 

= M3 

= 1. ODO 

= M3 
JOINT TOO 

= -1. ODO 
= -RAG2(3) 
= RAG2(2) 

= 1. ODO 

= RBG3(3) 

= -RBG3(2) 

= -1. ODO 
= RAG2( 3) 
= -RAG2( 1) 
= 1. ODO 

= -RBG3C3) 
= RBG3C1) 

= -1. ODO 
= -RAG2C2) 
= RAG2C1) 
= 1. ODO 

= RBG3C2) 
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■>v 



* 



* 



VoWoW? 

1051 



MATA( 24 , 26) = -RBG3(1) 
SUM OF MOMENTS EQUATIONS 
X DIRECTION 
MATA( 25,20) 

MATA(25 ,21) 

MATA( 25,25) 

MATA( 25,26) 

MATA( 25,27) 

Y DIRECTION 

MATA( 26,19) = -RBG3(3) 
MATA( 26,21) 

MATA( 26,25) 

MATA( 26 , 26) 

MATA( 26,27) 

Z DIRECTION 
MATA( 27,19) 

MATA( 27,20) 

MATA( 27,25) 

MATA( 27 , 26) 

MATA( 27,27) 

FIRST LINK IS 



RBG3( 3) 
-RBG3( 2) 
-IXXT( 3) 
IXYT( 3) 
IXZT( 3) 



RBG3( 1) 
IXYT( 3) 
-IYYT( 3) 
IYZT( 3) 

RBG3( 2 ) 
-RBG3( 1) 
IXZT( 3) 
IYZT( 3) 
-IZZT( 3) 
CONSTRAINED 



TO ROTATE IN Z DIRECTION ***** 



0. 0D0 
0. 0D0 



1. 0D0 
1. 0D0 
1. 0D0 
1. 0D0 
1. 0D0 



DO 1056 I = 4,8 
DO 1055 J = 1,27 
MATA( I , J) 

* MATA( J,I) 

1055 CONTINUE 

1056 CONTINUE 

MATA(4 ,4) 

MATA( 5,5) 
MATA(6,6) 

MATA( 7,7) 

MATA( 8,8) 

DO 1065 J = 1,27 

MATA( 18 ,J) = O.ODO 
MATA( 27 ,J) = 0. 0D0 

1065 CONTINUE 

IZZT( 2) = IZZT( 2) + 
IZZT( 3) = IZZT( 3) + 
MATA( 9,9) 

MATA( 18,9) = - 

MATA( 18,18) = 

MATA( 27 , 9) = - 

MATA( 27,27) = 

* MULTIPLY MATA * MATD TO 

DO 758 J = 1,27 

SUM1 = 0. ODO 
DO 755 K = 1,27 
SUM1 = SUM1 

755 CONTINUE 

MATC(J) = SUM1 

758 CONTINUE 



M2 * (DSQRT(LCOGX(2)**2+LCOGY(2)**2))**2 
M3 * (DSQRT( LCOGX( 3)**2+LCOGY( 3)**2) )**2 
(IZZT(1)+IZZT(2)+IZZT(3)) 

1. ODO 
1. ODO 
1. ODO 
1. ODO 

OBTAIN TORQUES 



+ MATA( J,K) * MATD(K) 



T2Z = -MATC( 27) 

T2Y = -MATC( 26) 

T2X = -MATC( 25) 

T1Z = T2Z - MATC( 18) 
T1Y = T2Y - MATC (17) 
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* 

1077 



T1X = T2X 

TOZ = TlZ 

TOY = T1Y 

TOX = T1X 

ENTER KNOWNS INTO 



- MATC( 16) 

- MATC( 9 ) 

- MATC( 8) 

- MATCC7) 
MATB 






1107 



MATB(l) 


= 


0. ODO 


MATB( 2) 


= 


0. ODO 


MATB( 3) 


= 


-WG1 


MATB(4) 


= 


AXO - MICO 


MATB(5) 


= 


AYO - MJCO 


MATB( 6) 




AZO - MKCO 


MATB( 7 ) 


= 


T1X - TOX 


MATB( 8) 


— 


T1Y - TOY 


MATB( 9) 


= 


TlZ - TOZ 


MATB(IO) 


— 


0. ODO 


MATB(ll) 


— 


0. ODO 


MATB( 12) 


— 


-WG2 


MATB( 13) 


= 


MIC1 - MIC2 


MATB( 14) 


= 


MJC1 - MJC2 


MATB( 15) 


= 


MKC1 - MKC2 


MATB( 16) 


= 


T2X - T1X 


MATB( 17) 


= 


T2Y - T1Y 


MATB( 18) 


= 


T2Z - TlZ 


MATB( 19) 


— 


0. ODO 


MATB (20) 


= 


0. ODO 


MATB( 21) 


= 


-WG3 


MATB( 22) 


= 


MIC3 - MIC4 


MATB (23) 


= 


MJC3 - MJC4 


MATB (24) 


= 


MKC3 - MKC4 


MATB (25) 


— 


-T2X 


MATB( 26) 


= 


-T2Y 


MATB( 27) 


— 


-T2Z 


■IRST LINK 


IS 


CONSTRAINED ' 


DO 1107 I 


= 4,8 


MATB(I) 




= 0. ODO 



Z DIRECTION 



CONTINUE 
MATB (18) 
MATB(27) 



0. ODO 
0. ODO 



■jV-jV-jV'jV’jV 



* CALL EQUATION SOLVER PROGRAM FROM IMSL 

1112 CALL LEQT2F(MATA,M,N, IA,MATB , IDGT,WKAREA, IER) 

IF (IER. EQ.O) THEN 

GOTO 1133 

ELSE 

WRITE (*,1117) IER 
1117 FORMAT (15) 

DO 1127 I = 1, 27 
WRITE (*,1120) I 
1120 FORMAT (17) 

DO 1124 J = 1, 27, 3 

WRITE (*,1123) J,MATA(I,J) , J+1,MATA( I, J+l) , J+2,MATA( I, J+2) 

1123 FORMAT ( 15 ,F13. 5 , 15 ,F13. 5 , 15 ,F13. 5) 

1124 CONTINUE 

WRITE (*,1126) I ,MATB( I) 

1126 FORMAT (I3,F15.5) 

1127 CONTINUE 
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vrVcVf 

Vf 

1133 



* 

* 



*141 

* 

Vc 



■5? 

Vr 

•i; 

Vr 



1166 



* 



CALL ENDJOB 
END IF 

FIND LCOGX , LCOGY , LCOGZ , THETA VALUE S,WX,WY,WZ 
JOINT ZERO 

FXO = MATE ( 1 ) 

FYO = MATB( 2) 

FZO = MATB( 3) 

LINK ONE 

SINCE LINK1 IS CONSTRAIN TO ROTATE AROUND THE Z ONLY 



AX1 = 0. ODO 
AY1 = 0. ODO 
AZ1 = 0. ODO 
AX1 
VELX1 
LCOGX 1 
LCOGX( 1) 

AY1 
VELY1 
LCOGY 1 
LCOGY( 1) 

AZ1 
VELZ1 
LCOGZ 1 
LCOGZ ( 1) 



= MATB(4) 

= INTGRL( 0. , AX1) 

= INTGRI.(X1 , VELX1) 
= LCOGX 1 
= MATB(5) 

= INTGRL( 0. ,AY1) 

= INTGRL( Y1 , VELY1) 
= LCOGY 1 
= MATB( 6) 

= INTGRL(0. ,AZ1) 

= INTGRL( Z 1 , VELZ 1 ) 
= LCOGZ 1 



WD1X 


= 


MATB( 7) 


W1X 




INTGRL(0. ,WD1X) 


WDX(l) 


— 


WD1X 


Wl(l) 


= 


W1X 


WD1Y 


rr 


MATB(8) 


W1Y 


= 


INTGRL( 0. ,WD1Y) 


WDY(l) 


= 


WD1Y 


Wl( 2) 


= 


W1Y 


WD1Z 


= 


MATB(9) 


W1Z 


= 


INTGRL( 2. , WD 1 Z ) 


WDZ(l) 


= 


WD1Z 


W 1 ( 3 ) 


= 


W1Z 


THZ 


= 


INTGRL(0. ,W1Z) 


THZANG 


= 


THZ * RADEG 


COSTHZ 


= 


DCOS(THZ) 


SINTHZ 


— 


DSIN(THZ) 



IF THE 1ST LINK IS CONSTRAIN TO ROTATE IN THE Z DIRECTION ONLY 

* THE DIRECTION COSINE AND DIRECTION COSINE ANGLES ARE CONSTANT 

* AND DO NOT NEED TO BE CALCULATED 

* JOINT ONE 

1174 FX1 = MATB(IO) 

FY1 = MATB(ll) 

FZ1 = MATB( 12) 

** LINK TWO 

1178 AX 2 = MATB( 13) 

VELX2 = INTGRL( -0. 416 , AX2) 

LC0GX2 = INTGRL( X2 , VELX2 ) 

LCOGX( 2) = LC0GX2 

AY2 = MATB(14) 

VELY2 = INTGRL( 0. ,AY2) 

LC0GY2 = INTGRL( Y2 , VELY2) 

LCOGY( 2) = LC0GY2 
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* 

1215 



•sV 

1220 



1224 



AZ2 


= 


MATB( 15) 


VELZ2 


= 


INTGRL( 0. 416 , AZ2) 


LC0GZ2 




INTGRL( Z2 , VELZ2) 


LCOGZ( 2 ) 


= 


LC0GZ2 


WD2X 


= 


MATB (16) 


W2X 


= 


INTGRL( 2. ,WD2X) 


WDX( 2) 


= 


WD2X 


W2( 1) 


= 


W2X 


WD2Y 


= 


MATB( 17) 


W2Y 


= 


INTGRL(0. ,WD2Y) 


WD Y ( 2 ) 


= 


WD2Y 


W2( 2 ) 


= 


W2Y 


WD2Z 


= 


MATB (18) 


W2Z 


= 


INTGRL( 2. ,WD2Z) 


W2( 3) 


— 


W2Z 


WDZ( 2) 


= 


WD2Z 



GET THE DIRECTION COSINES FOR THE LINK TWO 
DRCSX( 2) = ( LCOGX2 - JX1) / LNCOG2 

DRCSY( 2) = ( LCOGY2 - JY1) / LNCOG2 

DRCSZ( 2) = ( LCOGZ2 - JZ1) / LNCOG2 

AMP = DSQRT(DRCSX(2)*DRCSX(2)+DRCSY(2)*DRCSY(2)+. . . 

DRCSZ( 2)*DRCSZ( 2) ) 

DRCSX( 2) = DRCSX(2)/AMP 
DRCSY( 2) = DRCSY( 2)/AMP 
DRCSZ( 2) = DRCSZ( 2)/AMP 
DRCANX( 2) = DACOS(DRCSX( 2) ) * RADEG 
DRCANY( 2) = DACOS(DRCSY( 2) ) * RADEG 
DRCANZ( 2) = DACOS(DRCSZ( 2) ) * RADEG 
JOINT LOCATION 

JX2 = JX1 + LINKL2 * DRCSX(2) 

JY2 = JY1 + LINKL2 * DRCSY(2) 

JZ2 = JZ1 + LINKL2 * DRCSZ(2) 

JOINT TWO 

FX2 = MATB( 19) 

FY2 = MATB( 20) 

FZ2 = MATB (21) 

LINK THREE 



AX 3 




MATB(22) 


VELX3 




INTGRL( - 1. 282 , AX3) 


LC0GX3 


= 


INTGRL(X3 , VELX3) 


LCOGX( 3) 


= 


LC0GX3 


AY3 


= 


MATB( 23) 


VELY3 


= 


INTGRL(0. , AY3) 


LC0GY3 


= 


INTGRL( Y3 , VELY3) 


LCOGY( 3) 


= 


LC0GY3 


AZ3 


= 


MATB (24) 


VELZ3 


= 


INTGRL( 1. 282 , AZ3) 


LC0GZ3 


= 


INTGRL( Z3 , VELZ3 ) 


LCOGZ( 3) 


= 


LC0GZ3 


WD3X 


= 


MATB (25) 


W3X 


= 


INTGRL(2. ,WD3X) 


WDX( 3) 


= 


WD3X 


W3( 1) 


= 


W3X 


WD3Y 


— 


MATB( 26) 


W3Y 




INTGRL(0. ,WD3Y) 
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WDY( 3) = WD3Y 

W3( 2) = W3Y 

WD3Z = MATB (27) 

W3Z = INTGRL( 2. ,WD3Z) 

WDZ( 3) = WD3Z 

W3( 3) = W3Z 

* CALC DIRECTIONAL COSINES FOR LINK THREE 
1249 DRCSX( 3) = ( LCOGX3 - JX2) / LNCOG3 

DRCSY( 3) = (LCOGY3 - JY2) / LNCOG3 
DRCSZ( 3) = (LCOGZ3 - JZ2) / LNCOG3 
AMP = DSQRT(DRCSX(3)*DRCSX(3)+DRCSY(3)*DRCSY(3)+. . . 

DRCSZ( 3)*DRCSZ( 3) ) 

DRCSX( 3) = DRCSX(3)/AMP 

DRCSY( 3) = DRCSY( 3) /AMP 

DRCSZ(3) = DRCSZ( 3)/AMP 

DRCANX( 3) = DACOS( DRCSXC 3 ) ) * RADEG 

DRCANY( 3) = DACOS(DRCSY( 3) ) * RADEG 

DRCANZ( 3) = DACOS(DRCSZ( 3) ) * RADEG 

* TIP LOCATION 

1261 TIPX = JX2 + LINKL3 * DRCSX(3) 

TIPY = JY2 + LINKL3 * DRCSY(3) 

TIPZ = JZ2 + LINKL3 * DRCSZ(3) 

* FIND THE ANGLE BETWEEN THE LIMKS 
1265 ANG23X = DRCANX(2) - DRCANX(3) 

ANG23Y = DRC ANY ( 2 ) - DRCANY( 3) 

ANG23Z = DPCANZ( 2) - DRCANZ(3) 

ANG12X = DRCANX(l) - DRCANX(2) 

ANG12Y = DRC ANY ( 1 ) - DRC ANY ( 2 ) 

ANG12Z = DRCANZ(l) - DRCANZ(2) 

ANG12 = DRCANZ( 2) - DRCANZ(l) 

ANG23 = ANG23X + ANG23Y + ANG23Z 

* CLACULATE THE ERROR BETWEEN KNOWN TIP POSITION AND CALCULATED 

ERROR1 = ABS ( LTIPX - TIPX ) / 0.867 * 100.0 

ERROR2 = ABS ( LTIPY - TIPY ) / 0.867 * 100.0 

ERROR3 = ABS ( LTIPZ - TIPZ ) / 0. 867 * 100. 0 

END 

STOP 

FORTRAN 

* SUBROUTINE TO COMPUTE THE CROSS PRODUCT OF TWO VECTORS 

SUBROUTINE CPROD( VECTA , VECTB , MI , M J , MK) 

1284 IMPLICIT REAL* 8 (A-Z) 

DIMENSION VECTA( 3) , VECTB( 3) 

MI = VECTA( 2) * VECTB( 3) - VECTA(3) * VECTB(2) 

MJ = VECTA( 3) * VECTB ( 1) - VECTA(l) * VECTB(3) 

MK = VECTA(l) * VECTBC2) - VECTA(2) * VECTB(l) 

RETURN 

END 
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